旋翼回收火箭系列博客3——控制系统设计（PX4火箭）

2023-05-16

绪论

为了缩短研制周期和提高产品可靠性，本系统采用商用开源自动驾驶仪PX4，实现旋翼空中展开并回收的功能。PX4是全球最为成熟的开源自动驾驶仪，可实现自动起飞、降落、执行航点等基本任务。然而此次火箭比赛要求能够实现自动展开和降落至指定区域，所以需要对现有飞行硬件进行改造。其困难和解决方案如下：

困难：

火箭发射过载一般大于5g，商用GPS会自动锁定，导致飞控无法解析出全球定位信息。这种情况下，自动飞行的功能将全部丧失，会导致整个飞行任务的失败；
火箭要求自动飞行，地面人员不能参与手动控制。然而对于一般的旋翼飞机来讲，为了保证安全，会要求有遥控器连接到飞控，以便随时切换回手动飞行。所以解除掉手动飞行的限制也是关键之一；
火箭要求达到最高点以后再展开机械臂，这就要求首先飞控输出相关的作动信号，然后飞机再进行螺旋桨的起旋，最后执行降落至指定区域。

解决方案：

针对GPS上锁问题，我们采用国产高精度、高动态特性的GPS模块，保证火箭起飞过程中的定位连续性和可靠性；
针对解除手动控制，应当对飞控源码进行更改，并时刻让飞控保持在offboard模式下，接受来自地面站的指令；
对于上述复杂功能，飞控自身是难以完成的，因此需要借助上位机的协助。为了减小火箭质量、简化系统复杂度，我们采用地面电脑作为上位机，实时向飞控发送指令。协议采用MAVLINK，使用MAVSDK进行开发。
下面将针对硬件和软件两方面进行详细阐述：

硬件

飞控

采用CUAV公司开发生产的CUAV V5 Nano作为核心控制系统，它是PX4的FMU V5版本，因此具有强大的数据处理能力和多冗余系统，保证了飞行任务的可靠性和稳定性。还有一个原因是它尺寸极小，可以为模型火箭节省空间和质量。其外形如下图所示。

在这里插入图片描述

数传

飞控需要接受地面的实时指令，因此需要高带宽、低时延的通讯方式。我们采用CUAV公司开发生产的P9超远程数传，其最远传输距离达30KM，支持902-928MHZ的跳频传输，输出功率达1W。其外形及接口定义如图所示。
在这里插入图片描述

GPS模块

使用和芯星通公司专门针对弹载领域应用推出的高性能小型化卫星导航定位OEM板卡。支持GPSL1、GLOL1、BD2B1、B2、B3信号，使用RS232串口输出，尺寸仅38mm。热启动时间小于6秒，重启捕获时间小于2秒；轴向速度在2000m/s，加速度20～30g以下能保持良好的定位精度。水平和高程精度均小于10m，测速精度小于0.2m/s。其外形尺寸如图所示：

在这里插入图片描述

软件部分

自动驾驶仪 PX4

采用最新的PX4 V12 beta版本，此版本支持最新的MAVLINK拓展功能和取消起飞怠速功能。在Ubuntu18.0系统上对固件进行编译，然后通过USB烧写至飞控。由于上述GPS板卡输出格式与PX4内部格式不一致，需要进行源码的更改。（PX4无法识别标准NMEA格式GPS，在最新的V13版本上已得到解决）
火箭实物飞行成本高、危险性大，因此必要的仿真验证和室内测试是必要的。采用PX4 sitl+Gazebo的方式进行仿真，以验证程序代码的完整性，仿真过程如下图。其后进行室内测试，保证数据获取完整、相关硬件能够按照规定启动。

在这里插入图片描述

通讯协议 MAVSDK

采用轻量级通讯协议MAVLINK进行地面电脑与火箭上飞控的串口通讯，使用MAVSDK进行开发。MAVSDK是针对MAVLINK的开发套件，封装了常用的功能，包括数据获取、自动飞行等。所有的代码均已开源，详见个人Github主页
在软件中，其飞行时序如下：

时间/秒	动作
00:00	检测过载超过3g时，触发计时
05:00	打开降落伞，将火箭拉直
06:00	折叠机构展开
07:00	螺旋桨起旋
07:30	开始自动飞向目标点上空
unknow	启动自动着陆
unknow	降落后飞行器上锁

代码

/*************************Summary***************************
Function: Rocket recycle throught MAVSDK
1. Connect to PX4 through Serial serial:///dev/ttyUSB0:57600;
2. Check the overload of PX4, if it is over 3g, start timing;
3. 00:05:00, fired the parachute;
3. 00:06:00, unfold the arms;
4. 00:07:00, armed the vehicle;
5. 00:07:30, started the return mission;
6. after the mission, disarmed the vehicle;
Author: YDM <1779876755@qq.com>
************************************************************/

/*****************Subscribe Message Frequency*******************
        NAME          Frequency            Rate Function            Callback
LOCAL_POSITION_NED      1HZ     set_rate_position_velocity_ned  subscribe_position_velocity_ned
GLOBAL_POSITION_INT     1HZ     set_rate_position               subscribe_position
HIGHRES_IMU             5HZ     set_rate_imu                    subscribe_imu
***************************************************************/

/*****************Advertise Message Frequency*******************
        NAME          Frequency            Rate Function            Callback
LOCAL_POSITION_NED      1HZ     set_rate_position_velocity_ned  subscribe_position_velocity_ned
***************************************************************/

/*****************Heard file and Namespace*********************/
#include <chrono>
#include <cstdint>
#include <functional>
#include <iostream>
#include <future>
#include <memory>
#include <thread>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/mission/mission.h>
#include <mavsdk/plugins/telemetry/telemetry.h>


using namespace mavsdk;
using namespace std::placeholders; // for `_1`
using namespace std::chrono; // for seconds(), milliseconds()
using namespace std::this_thread; // for sleep_for()

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
/**************************************************************/

/**************************Exit Function*********************/
// Handles Action's result
inline void handle_action_err_exit(Action::Result result, const std::string& message);
// Handles Mission's result
inline void handle_mission_err_exit(Mission::Result result, const std::string& message);
// Handles Connection result
inline void handle_connection_err_exit(ConnectionResult result, const std::string& message);
/**************************************************************/

static Mission::MissionItem make_mission_item(
    double latitude_deg,
    double longitude_deg,
    float relative_altitude_m,
    float speed_m_s,
    bool is_fly_through,
    float gimbal_pitch_deg,
    float gimbal_yaw_deg,
    Mission::MissionItem::CameraAction camera_action);

/*************************How to use it************************/
void usage(std::string bin_name)
{
    std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " <connection_url>" << std::endl
              << "Connection URL format should be :" << std::endl
              << " For TCP : tcp://[server_host][:server_port]" << std::endl
              << " For UDP : udp://[bind_host][:bind_port]" << std::endl
              << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
              << "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
}

/*************************Main Function**************************/
int main(int argc, char** argv)
{
    // Initialing variable
    Mavsdk mavsdk;
    std::string connection_url;
    ConnectionResult connection_result;
    Telemetry::AccelerationFrd imu_current;
    float acc_x = 0.0;
    float acc_y = 0.0;
    float acc_z = 0.0;

/**************************Connecting the vehicle*********************************/

    if (argc == 2) {
        connection_url = argv[1];
        connection_result = mavsdk.add_any_connection(connection_url);
    } else {
        usage(argv[0]);
        return 1;
    }

    if (connection_result != ConnectionResult::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " << connection_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    std::cout << "Waiting to discover system..." << std::endl;
    auto prom = std::promise<std::shared_ptr<System>>{};
    auto fut = prom.get_future();

    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
        auto system = mavsdk.systems().back();

        if (system->has_autopilot()) {
            std::cout << "Discovered autopilot" << std::endl;

            // Unsubscribe again as we only want to find one system.
            mavsdk.subscribe_on_new_system(nullptr);
            prom.set_value(system);
        }
    });

    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds.
    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
        std::cout << ERROR_CONSOLE_TEXT << "No autopilot found, exiting." << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

    auto system = fut.get();

    auto telemetry = Telemetry{system};
    auto action = Action{system};
    auto mission = Mission(system);

/***********************************************************************************/

/**************************Subscribe Message and Callback***************************/

    // We want to listen to the altitude of the drone at 1 Hz in local coordinate system.
    const Telemetry::Result set_local_position_rate_result = telemetry.set_rate_position_velocity_ned(1.0);
    if (set_local_position_rate_result != Telemetry::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Setting rate failed:" << set_local_position_rate_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Set up callback to monitor altitude while the vehicle is in flight
    telemetry.subscribe_position_velocity_ned([](Telemetry::PositionVelocityNed local_position) {
        std::cout << TELEMETRY_CONSOLE_TEXT // set to blue
                  << "Local Altitude: " << local_position.position.down_m << " m"
                  << NORMAL_CONSOLE_TEXT // set to default color again
                  << std::endl;
    });

    // We want to listen to the altitude of the drone at 1 Hz in global coordinate system.
    const Telemetry::Result set_rate_result = telemetry.set_rate_position(1.0);
    if (set_rate_result != Telemetry::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Setting rate failed:" << set_rate_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Set up callback to monitor altitude while the vehicle is in flight
    telemetry.subscribe_position([](Telemetry::Position position) {
        std::cout << TELEMETRY_CONSOLE_TEXT // set to blue
                  << "Global Altitude: " << position.relative_altitude_m << " m"
                  << NORMAL_CONSOLE_TEXT // set to default color again
                  << std::endl;
    });

    // We want to listen to the acceleration of the drone at 5 Hz in body coordinate system(frd).
    const Telemetry::Result set_imu_rate_result = telemetry.set_rate_imu(5.0);
    if (set_imu_rate_result != Telemetry::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Setting rate failed:" << set_imu_rate_result
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Set up callback to monitor altitude while the vehicle is in flight
    telemetry.subscribe_imu([&acc_x, &acc_y, &acc_z](Telemetry::Imu imu) {
        acc_x = imu.acceleration_frd.forward_m_s2 ;
        acc_y = imu.acceleration_frd.right_m_s2 ;
        acc_z = imu.acceleration_frd.down_m_s2 ;
        // std::cout << TELEMETRY_CONSOLE_TEXT // set to blue
        //           << "ACC_X: " << acc_x << " m/s^2"
        //           << "ACC_Y: " << acc_y << " m/s^2"
        //           << "ACC_Z: " << acc_z << " m/s^2"
        //           << NORMAL_CONSOLE_TEXT // set to default color again
        //           << std::endl;

    });

/***********************************************************************************/

/********************************Flight Timing*************************************/
    // Check if vehicle is ready to arm
    // while (telemetry.health_all_ok() != true) {
    //     std::cout << "Vehicle is getting ready to arm" << std::endl;
    //     sleep_for(seconds(1));
    // }

/*************************Check for the fire signal**********************************/
    while(sqrt(acc_x * acc_x + acc_y * acc_y + acc_z * acc_z) < 15)
    {
        sleep_for(std::chrono::milliseconds(200));
        std::cout << "Waiting for launch..." << std::endl;
    }
/********************************Unfold Arms*************************************/
    std::cout << "Sleeping for 10 seconds..." << std::endl;
    sleep_for(seconds(10));

    std::cout << "Unfolding Arms...\n";
    const Action::Result set_actuator0_result = action.set_actuator(1, 1.0);

    if (set_actuator0_result != Action::Result::Success) {
        std::cerr << "Setting actuator failed:" << set_actuator0_result << '\n';
        return 1;
    }
    std::cout << "Unfolded Arms !!!\n";
/*************************Arm vehicle**********************************/
    std::cout << "Sleeping for 2 seconds..." << std::endl;
    sleep_for(seconds(2));
    std::cout << "Arming..." << std::endl;
    const Action::Result arm_result = action.arm();

    if (arm_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << arm_result << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

/********************************Fire Parachute*************************************/
    std::cout << "Sleeping for 1 seconds..." << std::endl;
    sleep_for(seconds(1));

    std::cout << "Firing Parachute...\n";
    const Action::Result set_actuator1_result = action.set_actuator(2, 0.8);

    if (set_actuator1_result != Action::Result::Success) {
        std::cerr << "Setting actuator failed:" << set_actuator1_result << '\n';
        return 1;
    }
    std::cout << "Fired Parachute !!!\n";

/***************************Started Mission***************************************/
{
    std::cout << "Creating and uploading mission" << std::endl;

    std::vector<Mission::MissionItem> mission_items;

    mission_items.push_back(make_mission_item(
        47.398570327054473,
        8.5459490218639658,
        0.0f,
        5.0f,
        false,
        20.0f,
        60.0f,
        Mission::MissionItem::CameraAction::None));

    {
        std::cout << "Uploading mission..." << std::endl;
        // We only have the upload_mission function asynchronous for now, so we wrap it using
        // std::future.
        auto prom = std::make_shared<std::promise<Mission::Result>>();
        auto future_result = prom->get_future();
        Mission::MissionPlan mission_plan{};
        mission_plan.mission_items = mission_items;
        mission.upload_mission_async(
            mission_plan, [prom](Mission::Result result) { prom->set_value(result); });

        const Mission::Result result = future_result.get();
        if (result != Mission::Result::Success) {
            std::cout << "Mission upload failed (" << result << "), exiting." << std::endl;
            return 1;
        }
        std::cout << "Mission uploaded." << std::endl;
    }


    // Before starting the mission, we want to be sure to subscribe to the mission progress.
    mission.subscribe_mission_progress([](Mission::MissionProgress mission_progress) {
        std::cout << "Mission status update: " << mission_progress.current << " / "
                  << mission_progress.total << std::endl;
    });

    {
        std::cout << "Starting mission." << std::endl;
        auto prom = std::make_shared<std::promise<Mission::Result>>();
        auto future_result = prom->get_future();
        mission.start_mission_async([prom](Mission::Result result) {
            prom->set_value(result);
            std::cout << "Started mission." << std::endl;
        });

        const Mission::Result result = future_result.get();
        handle_mission_err_exit(result, "Mission start failed: ");
    }


    while (!mission.is_mission_finished().second) {
        sleep_for(seconds(1));
    }
}
    /**************************************************************/

    std::cout << "Landing..." << std::endl;
    const Action::Result land_result = action.land();
    if (land_result != Action::Result::Success) {
        std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << land_result << NORMAL_CONSOLE_TEXT
                  << std::endl;
        return 1;
    }

    // Check if vehicle is still in air
    while (telemetry.in_air()) {
        std::cout << "Vehicle is landing..." << std::endl;
        sleep_for(seconds(1));
    }
    std::cout << "Landed!" << std::endl;

    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
    sleep_for(seconds(3));
    std::cout << "Finished..." << std::endl;

    return 0;
}

inline void handle_action_err_exit(Action::Result result, const std::string& message)
{
    if (result != Action::Result::Success) {
        std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
        exit(EXIT_FAILURE);
    }
}

inline void handle_mission_err_exit(Mission::Result result, const std::string& message)
{
    if (result != Mission::Result::Success) {
        std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
        exit(EXIT_FAILURE);
    }
}

// Handles connection result
inline void handle_connection_err_exit(ConnectionResult result, const std::string& message)
{
    if (result != ConnectionResult::Success) {
        std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
        exit(EXIT_FAILURE);
    }
}

Mission::MissionItem make_mission_item(
    double latitude_deg,
    double longitude_deg,
    float relative_altitude_m,
    float speed_m_s,
    bool is_fly_through,
    float gimbal_pitch_deg,
    float gimbal_yaw_deg,
    Mission::MissionItem::CameraAction camera_action)
{
    Mission::MissionItem new_item{};
    new_item.latitude_deg = latitude_deg;
    new_item.longitude_deg = longitude_deg;
    new_item.relative_altitude_m = relative_altitude_m;
    new_item.speed_m_s = speed_m_s;
    new_item.is_fly_through = is_fly_through;
    new_item.gimbal_pitch_deg = gimbal_pitch_deg;
    new_item.gimbal_yaw_deg = gimbal_yaw_deg;
    new_item.camera_action = camera_action;
    return new_item;
}

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)