Pixhawk原生固件PX4之TAKEOFF的启动流程

欢迎交流~ 个人 Gitter 交流平台，点击直达：

---

以TAKEOFF为例说明PX4中一个飞行模式的启动流程

• 众所周知由遥控器或者地面站决定Main state作为用户期望到达的飞行模式

• 然后有commander进行条件判断后输出一个Navigation state作为飞机最终的飞行模式

• 进而由navigator模块中的函数作为具体的飞行模式实现方法

• 具体的实现还得看位置控制

---

一、 首先从commander.cpp选中TAKEOFF开始

if (!strcmp(argv[1], "takeoff"))

这里需要注意的是

cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;

此结构体vehicle_command_s cmd被赋值，用户希望进入TAKEOFF模式。

这里省略对是否能切换到该模式的条件判断，直接进入TAKEOFF导航模式set_control_mode()

void
set_control_mode()
{
    /* set vehicle_control_mode according to set_navigation_state */
    ...
    switch (status.nav_state) {
    ...
    case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
        control_mode.flag_control_manual_enabled = false;
        control_mode.flag_control_auto_enabled = true;
        control_mode.flag_control_rates_enabled = true;
        control_mode.flag_control_attitude_enabled = true;
        control_mode.flag_control_rattitude_enabled = false;
        control_mode.flag_control_altitude_enabled = true;
        control_mode.flag_control_climb_rate_enabled = true;
        control_mode.flag_control_position_enabled = !status.in_transition_mode;
        control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
        control_mode.flag_control_acceleration_enabled = false;
        control_mode.flag_control_termination_enabled = false;
        break;
        ...
    }

二、 进入navigator_main函数

① 在主函数中完成对vehicle_command话题的检查更新后，根据commander中的模式选择为当前的结构体rep赋值

if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
/ TAKEOFF 起飞
    struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // takeoff_triplet

    // store current position as previous position and goal as next
    // 将现在的位置作为之前的位置存起来，将目标位置作为下一个航点
    rep->previous.yaw = get_global_position()->yaw;
    rep->previous.lat = get_global_position()->lat;
    rep->previous.lon = get_global_position()->lon;
    rep->previous.alt = get_global_position()->alt;

    rep->current.loiter_radius = get_loiter_radius();
    rep->current.loiter_direction = 1; // 顺时针
    rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
    rep->current.yaw = cmd.param4; // 偏航角(if magnetometer present), ignored without magnetometer

    /****
    MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
    Mission Param #1    Minimum pitch (if airspeed sensor present), desired pitch without sensor
    Mission Param #2    Empty
    Mission Param #3    Empty
    Mission Param #4    Yaw angle (if magnetometer present), ignored without magnetometer
    Mission Param #5    Latitude
    Mission Param #6    Longitude
    Mission Param #7    Altitude
    *****/

    if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
        rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
        rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
    } else {
            // If one of them is non-finite, reset both
            rep->current.lat = NAN;
            rep->current.lon = NAN;
            }

    rep->current.alt = cmd.param7;

    rep->previous.valid = true;
    rep->current.valid = true;
    rep->next.valid = false;

}

② 根据commander设定的导航状态进入相应导航模式

switch (_vstatus.nav_state) {
...
    case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
        _pos_sp_triplet_published_invalid_once = false;
        _navigation_mode = &_takeoff; // takeoff模式
        break;
...
}

③ 遍历导航模式，并为每个模式设置active/inactive

 /* iterate through navigation modes and set active/inactive for each */
// 遍历导航模式，并为每个模式设置active/inactive
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}

由②已经知道_navigation_mode = &_takeoff，所以这里run激活的是takeoff飞行模式。其中

_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_dataLinkLoss;
_navigation_mode_array[4] = &_engineFailure;
_navigation_mode_array[5] = &_gpsFailure;
_navigation_mode_array[6] = &_rcLoss;
_navigation_mode_array[7] = &_takeoff;
_navigation_mode_array[8] = &_land;
_navigation_mode_array[9] = &_follow_target;

注意： navigator模块中每一种模式都对应有on_inactive 、on_activation和on_active三个模式，由NavigatorMode::run(bool active)选择

void
NavigatorMode::run(bool active)
{
    if (active) {
        if (_first_run) { // 首次运行
            /* first run */
            _first_run = false;
            /* Reset stay in failsafe flag */
            _navigator->get_mission_result()->stay_in_failsafe = false;
            _navigator->set_mission_result_updated();
            /* 当模式激活时此函数会被调用一次，pos_sp_triplet必须在此初始化 */
            on_activation(); // 运行我

        } else { 
            /* 模式已激活时调用此函数 */
            on_active(); // 实际功能实现
        }

    } else {
        /* 模式未激活时调用此函数 */
        _first_run = true;
        // 
        on_inactive();
    }
}

由此可见一个模式的启用在navigator中是先经过on_activation()初始化，然后在on_active完成功能实现。

三、 takeoff过程

TAKEOFF其实没有什么好说的，必须有GPS才可以。

首先初始化home点的航点信息set_takeoff_position()，然后起飞MIS_TAKEOFF_ALT高度后悬停_navigator->set_can_loiter_at_sp(true)

---

                                          By Fantasy