pixhawk commander.cpp的飞行模式切换解读

commander.cpp逻辑性太强了，涉及整个系统的运作，所以分别拆分成小块看

另此篇blog大部分是参考（Pixhawk原生固件解读）飞行模式，控制模式的思路，笔者重新整理一下

此部分探究是因为进入不了光流定点模式，于是查看commander.cpp飞行模式切换部分

流程是：

(1)sensors.cpp发布ORB_ID(manual_control_setpoint)

(2)commander.cpp里set_main_state_rc()函数里的main_state_transition()函数根据遥控信息和飞行器状态status_flags决定是否能更变internal_state->main_state

(3)commander.cpp里set_nav_state()函数根据internal_state->main_state和飞行器状态status_flags(传感器等硬件正常否)确定能否完成internal_state->main_state所指定的模式，若飞行器状态不行，则将模式跟新为status->nav_state

(4)commander.cpp里set_control_mode()函数根据status.nav_state确定control_mode.flag_xxx

1. 遥控器端

Firmware/src/modules/sensors/sensors.cpp发布ORB_ID(manual_control_setpoint)

[cpp] view plain copy

1. /* only publish manual control if the signal is still present */  
2. if (!signal_lost) {  
3.   
4.     /* initialize manual setpoint */  
5.     struct manual_control_setpoint_s manual = {};  
6.     /* set mode slot to unassigned */  
7.     manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;  
8.     /* set the timestamp to the last signal time */  
9.     manual.timestamp = rc_input.timestamp_last_signal;  
10.   
11.     /* limit controls */  
12.     manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);  
13.     manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);  
14.     manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);  
15.     manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);  
16.     manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);  
17.     manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);  
18.     manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);  
19.     manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);  
20.     manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);  
21.     manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);  
22.   
23.     if (_parameters.rc_map_flightmode > 0) {  
24.   
25.         /* the number of valid slots equals the index of the max marker minus one */  
26.         const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;  
27.   
28.         /* the half width of the range of a slot is the total range 
29.          * divided by the number of slots, again divided by two 
30.          */  
31.         const float slot_width_half = 2.0f / num_slots / 2.0f;  
32.   
33.         /* min is -1, max is +1, range is 2. We offset below min and max */  
34.         const float slot_min = -1.0f - 0.05f;  
35.         const float slot_max = 1.0f + 0.05f;  
36.   
37.         /* the slot gets mapped by first normalizing into a 0..1 interval using min 
38.          * and max. Then the right slot is obtained by multiplying with the number of 
39.          * slots. And finally we add half a slot width to ensure that integer rounding 
40.          * will take us to the correct final index. 
41.          */  
42.         manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /  
43.                      (slot_max - slot_min)) + (1.0f / num_slots));  
44.   
45.         if (manual.mode_slot >= num_slots) {  
46.             manual.mode_slot = num_slots - 1;  
47.         }  
48.     }  
49.   
50.     /* mode switches */  
51.     manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,  
52.                  _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);  
53.     manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,  
54.                   _parameters.rc_rattitude_th,  
55.                   _parameters.rc_rattitude_inv);  
56.     manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,  
57.                    _parameters.rc_posctl_inv);  
58.     manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,  
59.                    _parameters.rc_return_inv);  
60.     manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,  
61.                    _parameters.rc_loiter_inv);  
62.     manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,  
63.                  _parameters.rc_acro_inv);  
64.     manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,  
65.                  _parameters.rc_offboard_th, _parameters.rc_offboard_inv);  
66.     manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,  
67.                  _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);  
68.   
69.     /* publish manual_control_setpoint topic */  
70.     if (_manual_control_pub != nullptr) {  
71.         orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);  
72.   
73.     } else {  
74.         _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);  
75.     }  

commander的主程序中

[cpp] view plain copy

1. /* RC input check */  
2. if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&  
3.     (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {  
4.     /* handle the case where RC signal was regained */  
5.     /* 处理信号失而复得的情况 */  
6.     if (!status_flags.rc_signal_found_once) {  
7.         status_flags.rc_signal_found_once = true;  
8.         status_changed = true;  
9.   
10.     } else {  
11.         if (status.rc_signal_lost) {  
12.             mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums",  
13.                          (hrt_absolute_time() - rc_signal_lost_timestamp) / 1000);  
14.             status_changed = true;  
15.         }  
16.     }  
17.   
18.     status.rc_signal_lost = false;  
19.   
20.     /* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm 
21.      * do it only for rotary wings in manual mode or fixed wing if landed */  
22.     /* 检查油门杆在左下角的位置&&在手动&&(Rattitude||AUTO_READY mode||ASSIST mode and landed，如果是，则上锁 
23.     if ((status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && 
24.         (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && 
25.         (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || 
26.             internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || 
27.             internal_state.main_state == commander_state_s::MAIN_STATE_STAB || 
28.             internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || 
29.             land_detector.landed) && 
30.         sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { 
31.  
32.         if (stick_off_counter > rc_arm_hyst) { 
33.             /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */  
34.             arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :  
35.                                vehicle_status_s::ARMING_STATE_STANDBY_ERROR);  
36.             arming_ret = arming_state_transition(&status,  
37.                                  &battery,  
38.                                  &safety,  
39.                                  new_arming_state,  
40.                                  &armed,  
41.                                  true /* fRunPreArmChecks */,  
42.                                  &mavlink_log_pub,  
43.                                  &status_flags,  
44.                                  avionics_power_rail_voltage);  
45.   
46.             if (arming_ret == TRANSITION_CHANGED) {  
47.                 arming_state_changed = true;  
48.             }  
49.   
50.             stick_off_counter = 0;  
51.   
52.         } else {  
53.             stick_off_counter++;  
54.         }  
55.   
56.     } else {  
57.         stick_off_counter = 0;  
58.     }  
59.   
60.     /* check if left stick is in lower right position and we're in MANUAL mode -> arm */  
61.     /* 检查油门杆在右下角的位置&&手动模式，如果是，则解锁 */  
62.     if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {  
63.         if (stick_on_counter > rc_arm_hyst) {  
64.   
65.             /* we check outside of the transition function here because the requirement 
66.              * for being in manual mode only applies to manual arming actions. 
67.              * the system can be armed in auto if armed via the GCS. 
68.              */  
69.   
70.             if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)  
71.                 && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)  
72.                 && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB)  
73.                 && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)  
74.                 && (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)  
75.                 && (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)  
76.                 ) {  
77.                 print_reject_arm("NOT ARMING: Switch to a manual mode first.");  
78.   
79.             } else if (!status_flags.condition_home_position_valid &&  
80.                         geofence_action == geofence_result_s::GF_ACTION_RTL) {  
81.                 print_reject_arm("NOT ARMING: Geofence RTL requires valid home");  
82.   
83.             } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {  
84.                 arming_ret = arming_state_transition(&status,  
85.                                      &battery,  
86.                                      &safety,  
87.                                      vehicle_status_s::ARMING_STATE_ARMED,  
88.                                      &armed,  
89.                                      true /* fRunPreArmChecks */,  
90.                                      &mavlink_log_pub,  
91.                                      &status_flags,  
92.                                      avionics_power_rail_voltage);  
93.   
94.                 if (arming_ret == TRANSITION_CHANGED) {  
95.                     arming_state_changed = true;  
96.                 } else {  
97.                     usleep(100000);  
98.                     print_reject_arm("NOT ARMING: Preflight checks failed");  
99.                 }  
100.             }  
101.             stick_on_counter = 0;  
102.   
103.         } else {  
104.             stick_on_counter++;  
105.         }  
106.   
107.     } else {  
108.         stick_on_counter = 0;  
109.     }  
110.   
111.     if (arming_ret == TRANSITION_CHANGED) {  
112.         if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {  
113.             mavlink_log_info(&mavlink_log_pub, "ARMED by RC");  
114.   
115.         } else {  
116.             mavlink_log_info(&mavlink_log_pub, "DISARMED by RC");  
117.         }  
118.   
119.         arming_state_changed = true;  
120.   
121.     } else if (arming_ret == TRANSITION_DENIED) {  
122.         /* 
123.          * the arming transition can be denied to a number of reasons: 
124.          *  - pre-flight check failed (sensors not ok or not calibrated) 
125.          *  - safety not disabled 
126.          *  - system not in manual mode 
127.          */  
128.         tune_negative(true);  
129.     }  
130.   
131.     /* evaluate the main state machine according to mode switches */  
132.     bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);  
133.     transition_result_t main_res = set_main_state_rc(&status);  
134.   
135.     /* play tune on mode change only if armed, blink LED always */  
136.     if (main_res == TRANSITION_CHANGED || first_rc_eval) {  
137.         tune_positive(armed.armed);  
138.         main_state_changed = true;  
139.   
140.     } else if (main_res == TRANSITION_DENIED) {  
141.         /* DENIED here indicates bug in the commander */  
142.         mavlink_log_critical(&mavlink_log_pub, "main state transition denied");  
143.     }  
144.   
145.     /* check throttle kill switch */  
146.     if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
147.         /* set lockdown flag */  
148.         /* 设置锁定标志 */  
149.         if (!armed.lockdown) {  
150.             mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH ENGAGED");  
151.         }  
152.         armed.lockdown = true;  
153.     } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {  
154.         if (armed.lockdown) {  
155.             mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF");  
156.         }  
157.         armed.lockdown = false;  
158.     }  
159.     /* no else case: do not change lockdown flag in unconfigured case */  
160.   
161. } else {  
162.     if (!status_flags.rc_input_blocked && !status.rc_signal_lost) {  
163.         mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);  
164.         status.rc_signal_lost = true;  
165.         rc_signal_lost_timestamp = sp_man.timestamp;  
166.         status_changed = true;  
167.     }  
168. }  

2.set_main_state_rc();函数内

2.1

orb_check(sp_man_sub, &updated);

if (updated) {

         orb_copy(ORB_ID(manual_control_setpoint),sp_man_sub, &sp_man);

}

sp_man.offboard_switch、sp_man.return_switch、sp_man.mode_slot、sp_man.mode_switch都会改变

2.2

[cpp] view plain copy

1. int new_mode =_flight_mode_slots[sp_man.mode_slot];  

_flight_mode_slots的定义：

[cpp] view plain copy

1. static int32_t_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];  

[cpp] view plain copy

1. static const int8_t MODE_SLOT_MAX = 6;  

也就是说_flight_mode_slots[]数组有6个元素，有6种模式可以选

赋值语句：

[cpp] view plain copy

1. param_get(_param_fmode_1,&_flight_mode_slots[0]);  
2. param_get(_param_fmode_2,&_flight_mode_slots[1]);  
3. param_get(_param_fmode_3,&_flight_mode_slots[2]);  
4. param_get(_param_fmode_4,&_flight_mode_slots[3]);  
5. param_get(_param_fmode_5,&_flight_mode_slots[4]);  
6. param_get(_param_fmode_6,&_flight_mode_slots[5]);  

来源是用户上位机配置

mode_slot的定义：

int8_t mode_slot;

mode_slot的赋值：

以上都是遥控信息的来源(先上位机用户定义哪个开关对应哪个模式，再直接切开关转变到相应的模式)通过这段程序还没看懂。

2.3

main_state_transition();根据遥控信息和飞行器状态status_flags决定是否能更变internal_state->main_state

[cpp] view plain copy

1. transition_result_t  
2. main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,  
3.               status_flags_s *status_flags, struct commander_state_s *internal_state)  
4. {  
5.     transition_result_t ret = TRANSITION_DENIED;  
6.     /* transition may be denied even if the same state is requested because conditions may have changed */  
7.     switch (new_main_state) {  
8.     case commander_state_s::MAIN_STATE_MANUAL:  
9.     case commander_state_s::MAIN_STATE_ACRO:  
10.     case commander_state_s::MAIN_STATE_RATTITUDE:  
11.     case commander_state_s::MAIN_STATE_STAB:  
12.         ret = TRANSITION_CHANGED;  
13.         break;  
14.     case commander_state_s::MAIN_STATE_ALTCTL:  
15.         /* need at minimum altitude estimate */  
16.         /* TODO: add this for fixedwing as well */  
17.         if (!status->is_rotary_wing ||  
18.             (status_flags->condition_local_altitude_valid ||  
19.              status_flags->condition_global_position_valid)) {  
20.             ret = TRANSITION_CHANGED;  
21.         }  
22.         break;  
23.     case commander_state_s::MAIN_STATE_POSCTL:  
24.         /* need at minimum local position estimate */  
25.         if (status_flags->condition_local_position_valid ||  
26.             status_flags->condition_global_position_valid) {  
27.             ret = TRANSITION_CHANGED;  
28.         }  
29.         break;  
30.     case commander_state_s::MAIN_STATE_AUTO_LOITER:  
31.         /* need global position estimate */  
32.         if (status_flags->condition_global_position_valid) {  
33.             ret = TRANSITION_CHANGED;  
34.         }  
35.         break;  
36.     case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:  
37.     case commander_state_s::MAIN_STATE_AUTO_MISSION:  
38.     case commander_state_s::MAIN_STATE_AUTO_RTL:  
39.     case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:  
40.     case commander_state_s::MAIN_STATE_AUTO_LAND:  
41.         /* need global position and home position */  
42.         if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
43.             ret = TRANSITION_CHANGED;  
44.         }  
45.         break;  
46.     case commander_state_s::MAIN_STATE_OFFBOARD:  
47.         /* need offboard signal */  
48.         if (!status_flags->offboard_control_signal_lost) {  
49.             ret = TRANSITION_CHANGED;  
50.         }  
51.         break;  
52.     case commander_state_s::MAIN_STATE_MAX:  
53.     default:  
54.         break;  
55.     }  
56.     if (ret == TRANSITION_CHANGED) {  
57.         if (internal_state->main_state != new_main_state) {  
58.             main_state_prev = internal_state->main_state;  
59.             internal_state->main_state = new_main_state;  
60.         } else {  
61.             ret = TRANSITION_NOT_CHANGED;  
62.         }  
63.     }  
64.     return ret;  
65. }  

[cpp] view plain copy

1. transition_result_t  
2. set_main_state_rc(struct vehicle_status_s *status_local)  
3. {  
4.     /* set main state according to RC switches */  
5.     transition_result_t res = TRANSITION_DENIED;  
6.   
7.     // XXX this should not be necessary any more, we should be able to  
8.     // just delete this and respond to mode switches  
9.     /* if offboard is set already by a mavlink command, abort */  
10.     if (status_flags.offboard_control_set_by_command) {  
11.         return main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);  
12.     }  
13.   
14.     /* manual setpoint has not updated, do not re-evaluate it */  
15.     if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||  
16.         ((_last_sp_man.offboard_switch == sp_man.offboard_switch) &&  
17.          (_last_sp_man.return_switch == sp_man.return_switch) &&  
18.          (_last_sp_man.mode_switch == sp_man.mode_switch) &&  
19.          (_last_sp_man.acro_switch == sp_man.acro_switch) &&  
20.          (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&  
21.          (_last_sp_man.posctl_switch == sp_man.posctl_switch) &&  
22.          (_last_sp_man.loiter_switch == sp_man.loiter_switch) &&  
23.          (_last_sp_man.mode_slot == sp_man.mode_slot))) {  
24.   
25.         // update these fields for the geofence system  
26.   
27.         if (!rtl_on) {  
28.             _last_sp_man.timestamp = sp_man.timestamp;  
29.             _last_sp_man.x = sp_man.x;  
30.             _last_sp_man.y = sp_man.y;  
31.             _last_sp_man.z = sp_man.z;  
32.             _last_sp_man.r = sp_man.r;  
33.         }  
34.   
35.         /* no timestamp change or no switch change -> nothing changed */  
36.         return TRANSITION_NOT_CHANGED;  
37.     }  
38.   
39.     _last_sp_man = sp_man;  
40. /***********************第一个判断***********************/  
41.     /* offboard switch overrides main switch */  
42.     if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
43.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);  
44.   
45.         if (res == TRANSITION_DENIED) {  
46.             print_reject_mode(status_local, "OFFBOARD");  
47.             /* mode rejected, continue to evaluate the main system mode */  
48.   
49.         } else {  
50.             /* changed successfully or already in this state */  
51.             return res;  
52.         }  
53.     }  
54. /***********************第二个判断***********************/  
55.     /* RTL switch overrides main switch */  
56.     if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
57.         warnx("RTL switch changed and ON!");  
58.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);  
59.   
60.         if (res == TRANSITION_DENIED) {  
61.             print_reject_mode(status_local, "AUTO RTL");  
62.   
63.             /* fallback to LOITER if home position not set */  
64.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);  
65.         }  
66.   
67.         if (res != TRANSITION_DENIED) {  
68.             /* changed successfully or already in this state */  
69.             return res;  
70.         }  
71.   
72.         /* if we get here mode was rejected, continue to evaluate the main system mode */  
73.     }  
74. /***********************第三个判断***********************/  
75.     /* we know something has changed - check if we are in mode slot operation */  
76.     if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {  
77.   
78.         if (sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {  
79.             warnx("m slot overflow");  
80.             return TRANSITION_DENIED;  
81.         }  
82.   
83.         int new_mode = _flight_mode_slots[sp_man.mode_slot];  
84.   
85.         if (new_mode < 0) {  
86.             /* slot is unused */  
87.             res = TRANSITION_NOT_CHANGED;  
88.   
89.         } else {  
90.             res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
91.   
92.             /* ensure that the mode selection does not get stuck here */  
93.             int maxcount = 5;  
94.   
95.             /* enable the use of break */  
96.             /* fallback strategies, give the user the closest mode to what he wanted */  
97.             while (res == TRANSITION_DENIED && maxcount > 0) {  
98.   
99.                 maxcount--;  
100.   
101.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {  
102.   
103.                     /* fall back to loiter */  
104.                     new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;  
105.                     print_reject_mode(status_local, "AUTO MISSION");  
106.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
107.   
108.                     if (res != TRANSITION_DENIED) {  
109.                         break;  
110.                     }  
111.                 }  
112.   
113.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) {  
114.   
115.                     /* fall back to position control */  
116.                     new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;  
117.                     print_reject_mode(status_local, "AUTO RTL");  
118.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
119.   
120.                     if (res != TRANSITION_DENIED) {  
121.                         break;  
122.                     }  
123.                 }  
124.   
125.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) {  
126.   
127.                     /* fall back to position control */  
128.                     new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;  
129.                     print_reject_mode(status_local, "AUTO LAND");  
130.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
131.   
132.                     if (res != TRANSITION_DENIED) {  
133.                         break;  
134.                     }  
135.                 }  
136.   
137.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {  
138.   
139.                     /* fall back to position control */  
140.                     new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;  
141.                     print_reject_mode(status_local, "AUTO TAKEOFF");  
142.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
143.   
144.                     if (res != TRANSITION_DENIED) {  
145.                         break;  
146.                     }  
147.                 }  
148.   
149.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) {  
150.   
151.                     /* fall back to position control */  
152.                     new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;  
153.                     print_reject_mode(status_local, "AUTO FOLLOW");  
154.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
155.   
156.                     if (res != TRANSITION_DENIED) {  
157.                         break;  
158.                     }  
159.                 }  
160.   
161.                 if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {  
162.   
163.                     /* fall back to position control */  
164.                     new_mode = commander_state_s::MAIN_STATE_POSCTL;  
165.                     print_reject_mode(status_local, "AUTO HOLD");  
166.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
167.   
168.                     if (res != TRANSITION_DENIED) {  
169.                         break;  
170.                     }  
171.                 }  
172.   
173.                 if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {  
174.   
175.                     /* fall back to altitude control */  
176.                     new_mode = commander_state_s::MAIN_STATE_ALTCTL;  
177.                     print_reject_mode(status_local, "POSITION CONTROL");  
178.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
179.   
180.                     if (res != TRANSITION_DENIED) {  
181.                         break;  
182.                     }  
183.                 }  
184.   
185.                 if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {  
186.   
187.                     /* fall back to stabilized */  
188.                     new_mode = commander_state_s::MAIN_STATE_STAB;  
189.                     print_reject_mode(status_local, "ALTITUDE CONTROL");  
190.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
191.   
192.                     if (res != TRANSITION_DENIED) {  
193.                         break;  
194.                     }  
195.                 }  
196.   
197.                 if (new_mode == commander_state_s::MAIN_STATE_STAB) {  
198.   
199.                     /* fall back to manual */  
200.                     new_mode = commander_state_s::MAIN_STATE_MANUAL;  
201.                     print_reject_mode(status_local, "STABILIZED");  
202.                     res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);  
203.   
204.                     if (res != TRANSITION_DENIED) {  
205.                         break;  
206.                     }  
207.                 }  
208.             }  
209.         }  
210.   
211.         return res;  
212.     }  
213. /***********************第四个判断***********************/  
214.     /* offboard and RTL switches off or denied, check main mode switch */  
215.     switch (sp_man.mode_switch) {  
216.     case manual_control_setpoint_s::SWITCH_POS_NONE:  
217.         res = TRANSITION_NOT_CHANGED;  
218.         break;  
219.   
220.     case manual_control_setpoint_s::SWITCH_POS_OFF:     // MANUAL  
221.         if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
222.   
223.             /* manual mode is stabilized already for multirotors, so switch to acro 
224.              * for any non-manual mode 
225.              */  
226.             // XXX: put ACRO and STAB on separate switches  
227.             if (status.is_rotary_wing && !status.is_vtol) {  
228.                 res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state);  
229.             } else if (!status.is_rotary_wing) {  
230.                 res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);  
231.             } else {  
232.                 res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);  
233.             }  
234.   
235.         }  
236.         else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){  
237.             /* Similar to acro transitions for multirotors.  FW aircraft don't need a 
238.              * rattitude mode.*/  
239.             if (status.is_rotary_wing) {  
240.                 res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state);  
241.             } else {  
242.                 res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);  
243.             }  
244.         }else {  
245.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);  
246.         }  
247.   
248.         // TRANSITION_DENIED is not possible here  
249.         break;  
250.   
251.     case manual_control_setpoint_s::SWITCH_POS_MIDDLE:      // ASSIST  
252.         if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
253.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);  
254.   
255.             if (res != TRANSITION_DENIED) {  
256.                 break;  // changed successfully or already in this state  
257.             }  
258.   
259.             print_reject_mode(status_local, "POSITION CONTROL");  
260.         }  
261.   
262.         // fallback to ALTCTL  
263.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);  
264.   
265.         if (res != TRANSITION_DENIED) {  
266.             break;  // changed successfully or already in this mode  
267.         }  
268.   
269.         if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {  
270.             print_reject_mode(status_local, "ALTITUDE CONTROL");  
271.         }  
272.   
273.         // fallback to MANUAL  
274.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);  
275.         // TRANSITION_DENIED is not possible here  
276.         break;  
277.   
278.     case manual_control_setpoint_s::SWITCH_POS_ON:          // AUTO  
279.         if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {  
280.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);  
281.   
282.             if (res != TRANSITION_DENIED) {  
283.                 break;  // changed successfully or already in this state  
284.             }  
285.   
286.             print_reject_mode(status_local, "AUTO PAUSE");  
287.   
288.         } else {  
289.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);  
290.   
291.             if (res != TRANSITION_DENIED) {  
292.                 break;  // changed successfully or already in this state  
293.             }  
294.   
295.             print_reject_mode(status_local, "AUTO MISSION");  
296.   
297.             // fallback to LOITER if home position not set  
298.             res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);  
299.   
300.             if (res != TRANSITION_DENIED) {  
301.                 break;  // changed successfully or already in this state  
302.             }  
303.         }  
304.   
305.         // fallback to POSCTL  
306.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);  
307.   
308.         if (res != TRANSITION_DENIED) {  
309.             break;  // changed successfully or already in this state  
310.         }  
311.   
312.         // fallback to ALTCTL  
313.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);  
314.   
315.         if (res != TRANSITION_DENIED) {  
316.             break;  // changed successfully or already in this state  
317.         }  
318.   
319.         // fallback to MANUAL  
320.         res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);  
321.         // TRANSITION_DENIED is not possible here  
322.         break;  
323.   
324.     default:  
325.         break;  
326.     }  
327.   
328.     return res;  
329. }  

3.set_nav_state();根据internal_state->main_state和飞行器状态status_flags(传感器等硬件正常否)确定能否完成internal_state->main_state所指定的模式，若飞行器状态不行，则将模式跟新为status->nav_state。

internal_state->main_state包含下面14种：

[cpp] view plain copy

1. #define MAIN_STATE_MANUAL 0  
2. #define MAIN_STATE_ALTCTL 1  
3. #define MAIN_STATE_POSCTL 2  
4. #define MAIN_STATE_AUTO_MISSION 3  
5. #define MAIN_STATE_AUTO_LOITER 4  
6. #define MAIN_STATE_AUTO_RTL 5  
7. #define MAIN_STATE_ACRO 6  
8. #define MAIN_STATE_OFFBOARD 7  
9. #define MAIN_STATE_STAB 8  
10. #define MAIN_STATE_RATTITUDE 9  
11. #define MAIN_STATE_AUTO_TAKEOFF 10  
12. #define MAIN_STATE_AUTO_LAND 11  
13. #define MAIN_STATE_AUTO_FOLLOW_TARGET 12  
14. #define MAIN_STATE_MAX 13   

[cpp] view plain copy

1. <pre name="code" class="cpp">command.cpp中main函数  

[cpp] view plain copy

1. <pre name="code" class="cpp">/* now set navigation state according to failsafe and main state */  
2. bool nav_state_changed = set_nav_state(&status,  
3.                        &internal_state,  
4.                        (datalink_loss_enabled > 0),  
5.                        mission_result.finished,  
6.                        mission_result.stay_in_failsafe,  
7.                        &status_flags,  
8.                        land_detector.landed,  
9.                        (rc_loss_enabled > 0));  

[cpp] view plain copy

1. /** 
2.  * Check failsafe and main status and set navigation status for navigator accordingly 
3.  */  
4. bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,  
5.            const bool data_link_loss_enabled, const bool mission_finished,  
6.            const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled)  
7. {  
8.     navigation_state_t nav_state_old = status->nav_state;  
9.   
10.     bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);  
11.     status->failsafe = false;  
12.   
13.     /* evaluate main state to decide in normal (non-failsafe) mode */  
14.     switch (internal_state->main_state) {  
15.     case commander_state_s::MAIN_STATE_ACRO:  
16.     case commander_state_s::MAIN_STATE_MANUAL:  
17.     case commander_state_s::MAIN_STATE_RATTITUDE:  
18.     case commander_state_s::MAIN_STATE_STAB:  
19.     case commander_state_s::MAIN_STATE_ALTCTL:  
20.     case commander_state_s::MAIN_STATE_POSCTL:  
21.         /* require RC for all manual modes */  
22.         if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {  
23.             status->failsafe = true;  
24.   
25.             if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
26.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;  
27.             } else if (status_flags->condition_local_position_valid) {  
28.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
29.             } else if (status_flags->condition_local_altitude_valid) {  
30.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
31.             } else {  
32.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
33.             }  
34.   
35.         } else {  
36.             switch (internal_state->main_state) {  
37.             case commander_state_s::MAIN_STATE_ACRO:  
38.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;  
39.                 break;  
40.   
41.             case commander_state_s::MAIN_STATE_MANUAL:  
42.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;  
43.                 break;  
44.   
45.             case commander_state_s::MAIN_STATE_RATTITUDE:  
46.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;  
47.                 break;  
48.   
49.             case commander_state_s::MAIN_STATE_STAB:  
50.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;  
51.                 break;  
52.   
53.             case commander_state_s::MAIN_STATE_ALTCTL:  
54.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;  
55.                 break;  
56.   
57.             case commander_state_s::MAIN_STATE_POSCTL:  
58.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;  
59.                 break;  
60.   
61.             default:  
62.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;  
63.                 break;  
64.             }  
65.         }  
66.         break;  
67.   
68.     case commander_state_s::MAIN_STATE_AUTO_MISSION:  
69.   
70.         /* go into failsafe 
71.          * - if commanded to do so 
72.          * - if we have an engine failure 
73.          * - if we have vtol transition failure 
74.          * - depending on datalink, RC and if the mission is finished */  
75.   
76.         /* first look at the commands */  
77.         if (status->engine_failure_cmd) {  
78.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
79.         } else if (status_flags->data_link_lost_cmd) {  
80.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;  
81.         } else if (status_flags->gps_failure_cmd) {  
82.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;  
83.         } else if (status_flags->rc_signal_lost_cmd) {  
84.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;  
85.         } else if (status_flags->vtol_transition_failure_cmd) {  
86.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;  
87.   
88.         /* finished handling commands which have priority, now handle failures */  
89.         } else if (status_flags->gps_failure) {  
90.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;  
91.         } else if (status->engine_failure) {  
92.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
93.         } else if (status_flags->vtol_transition_failure) {  
94.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;  
95.         } else if (status->mission_failure) {  
96.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;  
97.   
98.         /* datalink loss enabled: 
99.          * check for datalink lost: this should always trigger RTGS */  
100.         } else if (data_link_loss_enabled && status->data_link_lost) {  
101.             status->failsafe = true;  
102.   
103.             if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
104.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;  
105.             } else if (status_flags->condition_local_position_valid) {  
106.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
107.             } else if (status_flags->condition_local_altitude_valid) {  
108.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
109.             } else {  
110.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
111.             }  
112.   
113.         /* datalink loss disabled: 
114.          * check if both, RC and datalink are lost during the mission 
115.          * or RC is lost after the mission is finished: this should always trigger RCRECOVER */  
116.         } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||  
117.                                (status->rc_signal_lost && mission_finished))) {  
118.             status->failsafe = true;  
119.   
120.             if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
121.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;  
122.             } else if (status_flags->condition_local_position_valid) {  
123.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
124.             } else if (status_flags->condition_local_altitude_valid) {  
125.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
126.             } else {  
127.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
128.             }  
129.   
130.         /* stay where you are if you should stay in failsafe, otherwise everything is perfect */  
131.         } else if (!stay_in_failsafe){  
132.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;  
133.         }  
134.         break;  
135.   
136.     case commander_state_s::MAIN_STATE_AUTO_LOITER:  
137.         /* go into failsafe on a engine failure */  
138.         if (status->engine_failure) {  
139.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
140.         /* also go into failsafe if just datalink is lost */  
141.         } else if (status->data_link_lost && data_link_loss_enabled) {  
142.             status->failsafe = true;  
143.   
144.             if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
145.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;  
146.             } else if (status_flags->condition_local_position_valid) {  
147.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
148.             } else if (status_flags->condition_local_altitude_valid) {  
149.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
150.             } else {  
151.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
152.             }  
153.   
154.         /* go into failsafe if RC is lost and datalink loss is not set up */  
155.         } else if (status->rc_signal_lost && !data_link_loss_enabled) {  
156.             status->failsafe = true;  
157.   
158.             if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {  
159.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;  
160.             } else if (status_flags->condition_local_position_valid) {  
161.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
162.             } else if (status_flags->condition_local_altitude_valid) {  
163.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
164.             } else {  
165.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
166.             }  
167.   
168.         /* don't bother if RC is lost if datalink is connected */  
169.         } else if (status->rc_signal_lost) {  
170.   
171.             /* this mode is ok, we don't need RC for loitering */  
172.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;  
173.         } else {  
174.             /* everything is perfect */  
175.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;  
176.         }  
177.         break;  
178.   
179.     case commander_state_s::MAIN_STATE_AUTO_RTL:  
180.         /* require global position and home, also go into failsafe on an engine failure */  
181.   
182.         if (status->engine_failure) {  
183.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
184.         } else if ((!status_flags->condition_global_position_valid ||  
185.                     !status_flags->condition_home_position_valid)) {  
186.             status->failsafe = true;  
187.   
188.             if (status_flags->condition_local_position_valid) {  
189.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
190.             } else if (status_flags->condition_local_altitude_valid) {  
191.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
192.             } else {  
193.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
194.             }  
195.         } else {  
196.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;  
197.         }  
198.         break;  
199.   
200.     case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:  
201.         /* require global position and home */  
202.   
203.         if (status->engine_failure) {  
204.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
205.         } else if (!status_flags->condition_global_position_valid) {  
206.             status->failsafe = true;  
207.   
208.             if (status_flags->condition_local_position_valid) {  
209.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
210.             } else if (status_flags->condition_local_altitude_valid) {  
211.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
212.             } else {  
213.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
214.             }  
215.         } else {  
216.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;  
217.         }  
218.         break;  
219.   
220.     case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:  
221.         /* require global position and home */  
222.   
223.         if (status->engine_failure) {  
224.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
225.         } else if ((!status_flags->condition_global_position_valid ||  
226.                     !status_flags->condition_home_position_valid)) {  
227.             status->failsafe = true;  
228.   
229.             if (status_flags->condition_local_position_valid) {  
230.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
231.             } else if (status_flags->condition_local_altitude_valid) {  
232.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
233.             } else {  
234.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
235.             }  
236.         } else {  
237.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;  
238.         }  
239.         break;  
240.   
241.     case commander_state_s::MAIN_STATE_AUTO_LAND:  
242.         /* require global position and home */  
243.   
244.         if (status->engine_failure) {  
245.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;  
246.         } else if ((!status_flags->condition_global_position_valid ||  
247.                     !status_flags->condition_home_position_valid)) {  
248.             status->failsafe = true;  
249.   
250.             if (status_flags->condition_local_altitude_valid) {  
251.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
252.             } else {  
253.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
254.             }  
255.         } else {  
256.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
257.         }  
258.         break;  
259.   
260.     case commander_state_s::MAIN_STATE_OFFBOARD:  
261.         /* require offboard control, otherwise stay where you are */  
262.         if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {  
263.             status->failsafe = true;  
264.   
265.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;  
266.         } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {  
267.             status->failsafe = true;  
268.   
269.             if (status_flags->condition_local_position_valid) {  
270.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;  
271.             } else if (status_flags->condition_local_altitude_valid) {  
272.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;  
273.             } else {  
274.                 status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;  
275.             }  
276.         } else {  
277.             status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;  
278.         }  
279.     default:  
280.         break;  
281.     }  
282.   
283.     return status->nav_state != nav_state_old;  
284. }  

4.set_control_mode();根据status.nav_state确定control_mode.flag_xxx

command.cpp的main函数中

[cpp] view plain copy

1. /* publish states (armed, control mode, vehicle status) at least with 5 Hz */  
2.     if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {  
3.     set_control_mode();  
4.     control_mode.timestamp = now;  
5.     orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);  
6.     status.timestamp = now;  
7.     orb_publish(ORB_ID(vehicle_status), status_pub, &status);  
8.     armed.timestamp = now;  
9.     /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */  
10.     if (safety.safety_switch_available) {  
11.         /* safety is off, go into prearmed */  
12.         armed.prearmed = safety.safety_off;  
13.         } else {  
14.         /* safety is not present, go into prearmed 
15.          * (all output drivers should be started / unlocked last in the boot process 
16.          * when the rest of the system is fully initialized) 
17.          */  
18.         armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);  
19.     }  
20.     orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);  
21. }  

status.nav_state可分为

[cpp] view plain copy

1. static const uint8_t NAVIGATION_STATE_MANUAL = 0;  
2. static const uint8_t NAVIGATION_STATE_ALTCTL = 1;  
3. static const uint8_t NAVIGATION_STATE_POSCTL = 2;  
4. static const uint8_t NAVIGATION_STATE_AUTO_MISSION = 3;  
5. static const uint8_t NAVIGATION_STATE_AUTO_LOITER = 4;  
6. static const uint8_t NAVIGATION_STATE_AUTO_RTL = 5;  
7. static const uint8_t NAVIGATION_STATE_AUTO_RCRECOVER = 6;  
8. static const uint8_t NAVIGATION_STATE_AUTO_RTGS = 7;  
9. static const uint8_t NAVIGATION_STATE_AUTO_LANDENGFAIL = 8;  
10. static const uint8_t NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9;  
11. static const uint8_t NAVIGATION_STATE_ACRO = 10;  
12. static const uint8_t NAVIGATION_STATE_UNUSED = 11;  
13. static const uint8_t NAVIGATION_STATE_DESCEND = 12;  
14. static const uint8_t NAVIGATION_STATE_TERMINATION = 13;  
15. static const uint8_t NAVIGATION_STATE_OFFBOARD = 14;  
16. 序只写到了这  
17. static const uint8_t NAVIGATION_STATE_STAB = 15;  
18. static const uint8_t NAVIGATION_STATE_RATTITUDE = 16;  
19. static const uint8_t NAVIGATION_STATE_AUTO_TAKEOFF = 17;  
20. static const uint8_t NAVIGATION_STATE_AUTO_LAND = 18;  
21. static const uint8_t NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19;  
22. static const uint8_t NAVIGATION_STATE_MAX = 20;  

需要确定的有

[cpp] view plain copy

1. control_mode.flag_control_manual_enabled = true;  
2. control_mode.flag_control_auto_enabled = false;  
3. control_mode.flag_control_rates_enabled = stabilization_required();  
4. control_mode.flag_control_attitude_enabled = stabilization_required();  
5. control_mode.flag_control_rattitude_enabled = false;  
6. control_mode.flag_control_altitude_enabled = false;  
7. control_mode.flag_control_climb_rate_enabled = false;  
8. control_mode.flag_control_position_enabled = false;  
9. control_mode.flag_control_velocity_enabled = false;  
10. control_mode.flag_control_acceleration_enabled = false;  
11. control_mode.flag_control_termination_enabled = false;  

[cpp] view plain copy

1. set_control_mode()  
2. {  
3.     /* set vehicle_control_mode according to set_navigation_state */  
4.     control_mode.flag_armed = armed.armed;  
5.     control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);  
6.     control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;  
7.     control_mode.flag_control_offboard_enabled = false;  
8.   
9.     switch (status.nav_state) {  
10.     case vehicle_status_s::NAVIGATION_STATE_MANUAL:  
11.         control_mode.flag_control_manual_enabled = true;  
12.         control_mode.flag_control_auto_enabled = false;  
13.         control_mode.flag_control_rates_enabled = stabilization_required();  
14.         control_mode.flag_control_attitude_enabled = stabilization_required();  
15.         control_mode.flag_control_rattitude_enabled = false;  
16.         control_mode.flag_control_altitude_enabled = false;  
17.         control_mode.flag_control_climb_rate_enabled = false;  
18.         control_mode.flag_control_position_enabled = false;  
19.         control_mode.flag_control_velocity_enabled = false;  
20.         control_mode.flag_control_acceleration_enabled = false;  
21.         control_mode.flag_control_termination_enabled = false;  
22.         break;  
23.   
24.     case vehicle_status_s::NAVIGATION_STATE_STAB:  
25.         control_mode.flag_control_manual_enabled = true;  
26.         control_mode.flag_control_auto_enabled = false;  
27.         control_mode.flag_control_rates_enabled = true;  
28.         control_mode.flag_control_attitude_enabled = true;  
29.         control_mode.flag_control_rattitude_enabled = true;  
30.         control_mode.flag_control_altitude_enabled = false;  
31.         control_mode.flag_control_climb_rate_enabled = false;  
32.         control_mode.flag_control_position_enabled = false;  
33.         control_mode.flag_control_velocity_enabled = false;  
34.         control_mode.flag_control_acceleration_enabled = false;  
35.         control_mode.flag_control_termination_enabled = false;  
36.         /* override is not ok in stabilized mode */  
37.         control_mode.flag_external_manual_override_ok = false;  
38.         break;  
39.   
40.     case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:  
41.         control_mode.flag_control_manual_enabled = true;  
42.         control_mode.flag_control_auto_enabled = false;  
43.         control_mode.flag_control_rates_enabled = true;  
44.         control_mode.flag_control_attitude_enabled = true;  
45.         control_mode.flag_control_rattitude_enabled = true;  
46.         control_mode.flag_control_altitude_enabled = false;  
47.         control_mode.flag_control_climb_rate_enabled = false;  
48.         control_mode.flag_control_position_enabled = false;  
49.         control_mode.flag_control_velocity_enabled = false;  
50.         control_mode.flag_control_acceleration_enabled = false;  
51.         control_mode.flag_control_termination_enabled = false;  
52.         break;  
53.   
54.     case vehicle_status_s::NAVIGATION_STATE_ALTCTL:  
55.         control_mode.flag_control_manual_enabled = true;  
56.         control_mode.flag_control_auto_enabled = false;  
57.         control_mode.flag_control_rates_enabled = true;  
58.         control_mode.flag_control_attitude_enabled = true;  
59.         control_mode.flag_control_rattitude_enabled = false;  
60.         control_mode.flag_control_altitude_enabled = true;  
61.         control_mode.flag_control_climb_rate_enabled = true;  
62.         control_mode.flag_control_position_enabled = false;  
63.         control_mode.flag_control_velocity_enabled = false;  
64.         control_mode.flag_control_acceleration_enabled = false;  
65.         control_mode.flag_control_termination_enabled = false;  
66.         break;  
67.   
68.     case vehicle_status_s::NAVIGATION_STATE_POSCTL:  
69.         control_mode.flag_control_manual_enabled = true;  
70.         control_mode.flag_control_auto_enabled = false;  
71.         control_mode.flag_control_rates_enabled = true;  
72.         control_mode.flag_control_attitude_enabled = true;  
73.         control_mode.flag_control_rattitude_enabled = false;  
74.         control_mode.flag_control_altitude_enabled = true;  
75.         control_mode.flag_control_climb_rate_enabled = true;  
76.         control_mode.flag_control_position_enabled = !status.in_transition_mode;  
77.         control_mode.flag_control_velocity_enabled = !status.in_transition_mode;  
78.         control_mode.flag_control_acceleration_enabled = false;  
79.         control_mode.flag_control_termination_enabled = false;  
80.         break;  
81.   
82.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:  
83.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:  
84.         /* override is not ok for the RTL and recovery mode */  
85.         control_mode.flag_external_manual_override_ok = false;  
86.         /* fallthrough */  
87.     case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:  
88.     case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:  
89.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:  
90.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:  
91.     case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:  
92.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:  
93.     case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:  
94.         control_mode.flag_control_manual_enabled = false;  
95.         control_mode.flag_control_auto_enabled = true;  
96.         control_mode.flag_control_rates_enabled = true;  
97.         control_mode.flag_control_attitude_enabled = true;  
98.         control_mode.flag_control_rattitude_enabled = false;  
99.         control_mode.flag_control_altitude_enabled = true;  
100.         control_mode.flag_control_climb_rate_enabled = true;  
101.         control_mode.flag_control_position_enabled = !status.in_transition_mode;  
102.         control_mode.flag_control_velocity_enabled = !status.in_transition_mode;  
103.         control_mode.flag_control_acceleration_enabled = false;  
104.         control_mode.flag_control_termination_enabled = false;  
105.         break;  
106.   
107.     case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:  
108.         control_mode.flag_control_manual_enabled = false;  
109.         control_mode.flag_control_auto_enabled = false;  
110.         control_mode.flag_control_rates_enabled = true;  
111.         control_mode.flag_control_attitude_enabled = true;  
112.         control_mode.flag_control_rattitude_enabled = false;  
113.         control_mode.flag_control_altitude_enabled = false;  
114.         control_mode.flag_control_climb_rate_enabled = true;  
115.         control_mode.flag_control_position_enabled = false;  
116.         control_mode.flag_control_velocity_enabled = false;  
117.         control_mode.flag_control_acceleration_enabled = false;  
118.         control_mode.flag_control_termination_enabled = false;  
119.         break;  
120.   
121.     case vehicle_status_s::NAVIGATION_STATE_ACRO:  
122.         control_mode.flag_control_manual_enabled = true;  
123.         control_mode.flag_control_auto_enabled = false;  
124.         control_mode.flag_control_rates_enabled = true;  
125.         control_mode.flag_control_attitude_enabled = false;  
126.         control_mode.flag_control_rattitude_enabled = false;  
127.         control_mode.flag_control_altitude_enabled = false;  
128.         control_mode.flag_control_climb_rate_enabled = false;  
129.         control_mode.flag_control_position_enabled = false;  
130.         control_mode.flag_control_velocity_enabled = false;  
131.         control_mode.flag_control_acceleration_enabled = false;  
132.         control_mode.flag_control_termination_enabled = false;  
133.         break;  
134.   
135.     case vehicle_status_s::NAVIGATION_STATE_DESCEND:  
136.         /* TODO: check if this makes sense */  
137.         control_mode.flag_control_manual_enabled = false;  
138.         control_mode.flag_control_auto_enabled = true;  
139.         control_mode.flag_control_rates_enabled = true;  
140.         control_mode.flag_control_attitude_enabled = true;  
141.         control_mode.flag_control_rattitude_enabled = false;  
142.         control_mode.flag_control_position_enabled = false;  
143.         control_mode.flag_control_velocity_enabled = false;  
144.         control_mode.flag_control_acceleration_enabled = false;  
145.         control_mode.flag_control_altitude_enabled = false;  
146.         control_mode.flag_control_climb_rate_enabled = true;  
147.         control_mode.flag_control_termination_enabled = false;  
148.         break;  
149.   
150.     case vehicle_status_s::NAVIGATION_STATE_TERMINATION:  
151.         /* disable all controllers on termination */  
152.         control_mode.flag_control_manual_enabled = false;  
153.         control_mode.flag_control_auto_enabled = false;  
154.         control_mode.flag_control_rates_enabled = false;  
155.         control_mode.flag_control_attitude_enabled = false;  
156.         control_mode.flag_control_rattitude_enabled = false;  
157.         control_mode.flag_control_position_enabled = false;  
158.         control_mode.flag_control_velocity_enabled = false;  
159.         control_mode.flag_control_acceleration_enabled = false;  
160.         control_mode.flag_control_altitude_enabled = false;  
161.         control_mode.flag_control_climb_rate_enabled = false;  
162.         control_mode.flag_control_termination_enabled = true;  
163.         break;  
164.   
165.     case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:  
166.         control_mode.flag_control_manual_enabled = false;  
167.         control_mode.flag_control_auto_enabled = false;  
168.         control_mode.flag_control_offboard_enabled = true;  
169.   
170.         /* 
171.          * The control flags depend on what is ignored according to the offboard control mode topic 
172.          * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) 
173.          */  
174.         control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||  
175.             !offboard_control_mode.ignore_attitude ||  
176.             !offboard_control_mode.ignore_position ||  
177.             !offboard_control_mode.ignore_velocity ||  
178.             !offboard_control_mode.ignore_acceleration_force;  
179.   
180.         control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||  
181.             !offboard_control_mode.ignore_position ||  
182.             !offboard_control_mode.ignore_velocity ||  
183.             !offboard_control_mode.ignore_acceleration_force;  
184.   
185.         control_mode.flag_control_rattitude_enabled = false;  
186.   
187.         control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&  
188.           !status.in_transition_mode;  
189.   
190.         control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||  
191.             !offboard_control_mode.ignore_position) && !status.in_transition_mode &&  
192.             !control_mode.flag_control_acceleration_enabled;  
193.   
194.         control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||  
195.             !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;  
196.   
197.         control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&  
198.           !control_mode.flag_control_acceleration_enabled;  
199.   
200.         control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||  
201.             !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;  
202.   
203.         break;  
204.   
205.     default:  
206.         break;  
207.     }  
208. }  

问题是不能切光流定点模式，按以上流程分析：

main_state_transition();里面

[cpp] view plain copy

1. case commander_state_s::MAIN_STATE_POSCTL:  
2.     /* need at minimum local position estimate */  
3.     if (status_flags->condition_local_position_valid ||  
4.         status_flags->condition_global_position_valid) {  
5.         ret = TRANSITION_CHANGED;  
6.     }  
7.     break;  

要想切换模式status_flags->condition_local_position_valid或者status_flags->condition_global_position_valid要为1

[cpp] view plain copy

1. /* update global position estimate */  
2. orb_check(global_position_sub, &updated);  
3. if (updated) {  
4.     /* position changed */  
5.     vehicle_global_position_s gpos;  
6.     orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);  
7.     /* copy to global struct if valid, with hysteresis */  
8.     // XXX consolidate this with local position handling and timeouts after release  
9.     // but we want a low-risk change now.  
10.     if (status_flags.condition_global_position_valid) {  
11.         if (gpos.eph < eph_threshold * 2.5f) {  
12.             orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);  
13.         }  
14.     } else {  
15.         if (gpos.eph < eph_threshold) {  
16.             orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);  
17.         }  
18.     }  
19. }  
20. /* update local position estimate */  
21. orb_check(local_position_sub, &updated);  
22. if (updated) {  
23.     /* position changed */  
24.     orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);  
25. }  
26. /* update attitude estimate */  
27. orb_check(attitude_sub, &updated);  
28. if (updated) {  
29.     /* position changed */  
30.     orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);  
31. }  
32. //update condition_global_position_valid  
33. //Global positions are only published by the estimators if they are valid  
34. if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {  
35.     //We have had no good fix for POSITION_TIMEOUT amount of time  
36.     if (status_flags.condition_global_position_valid) {  
37.         set_tune_override(TONE_GPS_WARNING_TUNE);  
38.         status_changed = true;  
39.         status_flags.condition_global_position_valid = false;  
40.     }  
41. } else if (global_position.timestamp != 0) {  
42.     // Got good global position estimate  
43.     if (!status_flags.condition_global_position_valid) {  
44.         status_changed = true;  
45.         status_flags.condition_global_position_valid = true;  
46.     }  
47. }  
48. /* update condition_local_position_valid and condition_local_altitude_valid */  
49. /* hysteresis for EPH */  
50. bool local_eph_good;  
51. if (status_flags.condition_local_position_valid) {  
52.     if (local_position.eph > eph_threshold * 2.5f) {  
53.         local_eph_good = false;  
54.     } else {  
55.         local_eph_good = true;  
56.     }  
57. } else {  
58.     if (local_position.eph < eph_threshold) {  
59.         local_eph_good = true;  
60.     } else {  
61.         local_eph_good = false;  
62.     }  
63. }  
64. check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid  
65.         && local_eph_good, &(status_flags.condition_local_position_valid), &status_changed);  
66. check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,  
67.         &(status_flags.condition_local_altitude_valid), &status_changed);  

其中void check_valid()原函数为

[cpp] view plain copy

1. check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)  
2. {  
3.     hrt_abstime t = hrt_absolute_time();  
4.     bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);  
5.     if (*valid_out != valid_new) {  
6.         *valid_out = valid_new;  
7.         *changed = true;  
8.     }  
9. }  

由此可知

status_flags.condition_global_position_valid和POSITION_TIMEOUT,global_position.timestamp有关

#definePOSITION_TIMEOUT  (1 * 1000 * 1000)// 考虑local或global的位置估计在1000毫秒无效

global_position.timestamp来自orb_copy(ORB_ID(vehicle_global_position),global_position_sub, &global_position);

ORB_ID(vehicle_global_position)来自位置估计

status_flags.condition_local_position_valid和local_position.timestamp,POSITION_TIMEOUT, local_position.xy_valid有关

#definePOSITION_TIMEOUT  (1 * 1000 * 1000)// 考虑local或global的位置估计在1000毫秒无效

local_position.timestamp, local_position.xy_valid来自orb_copy(ORB_ID(vehicle_local_position),local_position_sub, &local_position);

ORB_ID(vehicle_local_position)来自位置估计