歡迎交流~ 個人 Gitter 交流平臺，點選直達：

以TAKEOFF為例說明PX4中一個飛航模式的啟動流程

• 眾所周知由遙控器或者地面站決定Main state作為使用者期望到達的飛航模式

• 然後有commander進行條件判斷後輸出一個Navigation state作為飛機最終的飛航模式

• 進而由navigator模組中的函式作為具體的飛航模式實現方法

• 具體的實現還得看位置控制

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
 
;
        ...
    }

① 在主函式中完成對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其實沒有什麼好說的，必須有GPS才可以。

首先初始化home點的航點資訊set_takeoff_position()，然後起飛MIS_TAKEOFF_ALT高度後懸停_navigator->set_can_loiter_at_sp(true)

                                          By Fantasy