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 plaincopy
??if?(!signal_lost)?{??????????????struct?manual_control_setpoint_s?manual?=?{};????????????manual.mode_slot?=?manual_control_setpoint_s::MODE_SLOT_NONE;????????????manual.timestamp?=?rc_input.timestamp_last_signal;??????????????manual.y?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL,?-1.0,?1.0);??????manual.x?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH,?-1.0,?1.0);??????manual.r?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW,?-1.0,?1.0);??????manual.z?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE,?0.0,?1.0);??????manual.flaps?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS,?-1.0,?1.0);??????manual.aux1?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1,?-1.0,?1.0);??????manual.aux2?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2,?-1.0,?1.0);??????manual.aux3?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3,?-1.0,?1.0);??????manual.aux4?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4,?-1.0,?1.0);??????manual.aux5?=?get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5,?-1.0,?1.0);????????if?(_parameters.rc_map_flightmode?>?0)?{??????????????????????const?int?num_slots?=?manual_control_setpoint_s::MODE_SLOT_MAX;????????????????????????const?float?slot_width_half?=?2.0f?/?num_slots?/?2.0f;??????????????????????const?float?slot_min?=?-1.0f?-?0.05f;??????????const?float?slot_max?=?1.0f?+?0.05f;??????????????????????????manual.mode_slot?=?(((((_rc.channels[_parameters.rc_map_flightmode?-?1]?-?slot_min)?*?num_slots)?+?slot_width_half)?/???????????????????????(slot_max?-?slot_min))?+?(1.0f?/?num_slots));????????????if?(manual.mode_slot?>=?num_slots)?{??????????????manual.mode_slot?=?num_slots?-?1;??????????}??????}??????????????manual.mode_switch?=?get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE,?_parameters.rc_auto_th,???????????????????_parameters.rc_auto_inv,?_parameters.rc_assist_th,?_parameters.rc_assist_inv);??????manual.rattitude_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,????????????????????_parameters.rc_rattitude_th,????????????????????_parameters.rc_rattitude_inv);??????manual.posctl_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL,?_parameters.rc_posctl_th,?????????????????????_parameters.rc_posctl_inv);??????manual.return_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN,?_parameters.rc_return_th,?????????????????????_parameters.rc_return_inv);??????manual.loiter_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER,?_parameters.rc_loiter_th,?????????????????????_parameters.rc_loiter_inv);??????manual.acro_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO,?_parameters.rc_acro_th,???????????????????_parameters.rc_acro_inv);??????manual.offboard_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,???????????????????_parameters.rc_offboard_th,?_parameters.rc_offboard_inv);??????manual.kill_switch?=?get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,???????????????????_parameters.rc_killswitch_th,?_parameters.rc_killswitch_inv);??????????????if?(_manual_control_pub?!=?nullptr)?{??????????orb_publish(ORB_ID(manual_control_setpoint),?_manual_control_pub,?&manual);????????}?else?{??????????_manual_control_pub?=?orb_advertise(ORB_ID(manual_control_setpoint),?&manual);??????}??
commander的主程序中
[cpp] view plaincopy
??if?(!status_flags.rc_input_blocked?&&?sp_man.timestamp?!=?0?&&??????(hrt_absolute_time()?<?sp_man.timestamp?+?(uint64_t)(rc_loss_timeout?*?1e6f)))?{??????????????????if?(!status_flags.rc_signal_found_once)?{??????????status_flags.rc_signal_found_once?=?true;??????????status_changed?=?true;????????}?else?{??????????if?(status.rc_signal_lost)?{??????????????mavlink_log_info(&mavlink_log_pub,?"MANUAL?CONTROL?REGAINED?after?%llums",???????????????????????????(hrt_absolute_time()?-?rc_signal_lost_timestamp)?/?1000);??????????????status_changed?=?true;??????????}??????}????????status.rc_signal_lost?=?false;????????????????????????????????????????arming_state_t?new_arming_state?=?(status.arming_state?==?vehicle_status_s::ARMING_STATE_ARMED???vehicle_status_s::ARMING_STATE_STANDBY?:?????????????????????????????????vehicle_status_s::ARMING_STATE_STANDBY_ERROR);??????????????arming_ret?=?arming_state_transition(&status,???????????????????????????????????&battery,???????????????????????????????????&safety,???????????????????????????????????new_arming_state,???????????????????????????????????&armed,???????????????????????????????????true?,???????????????????????????????????&mavlink_log_pub,???????????????????????????????????&status_flags,???????????????????????????????????avionics_power_rail_voltage);????????????????if?(arming_ret?==?TRANSITION_CHANGED)?{??????????????????arming_state_changed?=?true;??????????????}????????????????stick_off_counter?=?0;????????????}?else?{??????????????stick_off_counter++;??????????}????????}?else?{??????????stick_off_counter?=?0;??????}????????????????????if?(sp_man.r?>?STICK_ON_OFF_LIMIT?&&?sp_man.z?<?0.1f?&&?status.rc_input_mode?!=?vehicle_status_s::RC_IN_MODE_OFF?)?{??????????if?(stick_on_counter?>?rc_arm_hyst)?{???????????????????????????????????if?((internal_state.main_state?!=?commander_state_s::MAIN_STATE_MANUAL)??????????????????&&?(internal_state.main_state?!=?commander_state_s::MAIN_STATE_ACRO)??????????????????&&?(internal_state.main_state?!=?commander_state_s::MAIN_STATE_STAB)??????????????????&&?(internal_state.main_state?!=?commander_state_s::MAIN_STATE_ALTCTL)??????????????????&&?(internal_state.main_state?!=?commander_state_s::MAIN_STATE_POSCTL)??????????????????&&?(internal_state.main_state?!=?commander_state_s::MAIN_STATE_RATTITUDE)??????????????????)?{??????????????????print_reject_arm("NOT?ARMING:?Switch?to?a?manual?mode?first.");????????????????}?else?if?(!status_flags.condition_home_position_valid?&&??????????????????????????geofence_action?==?geofence_result_s::GF_ACTION_RTL)?{??????????????????print_reject_arm("NOT?ARMING:?Geofence?RTL?requires?valid?home");????????????????}?else?if?(status.arming_state?==?vehicle_status_s::ARMING_STATE_STANDBY)?{??????????????????arming_ret?=?arming_state_transition(&status,???????????????????????????????????????&battery,???????????????????????????????????????&safety,???????????????????????????????????????vehicle_status_s::ARMING_STATE_ARMED,???????????????????????????????????????&armed,???????????????????????????????????????true?,???????????????????????????????????????&mavlink_log_pub,???????????????????????????????????????&status_flags,???????????????????????????????????????avionics_power_rail_voltage);????????????????????if?(arming_ret?==?TRANSITION_CHANGED)?{??????????????????????arming_state_changed?=?true;??????????????????}?else?{??????????????????????usleep(100000);??????????????????????print_reject_arm("NOT?ARMING:?Preflight?checks?failed");??????????????????}??????????????}??????????????stick_on_counter?=?0;????????????}?else?{??????????????stick_on_counter++;??????????}????????}?else?{??????????stick_on_counter?=?0;??????}????????if?(arming_ret?==?TRANSITION_CHANGED)?{??????????if?(status.arming_state?==?vehicle_status_s::ARMING_STATE_ARMED)?{??????????????mavlink_log_info(&mavlink_log_pub,?"ARMED?by?RC");????????????}?else?{??????????????mavlink_log_info(&mavlink_log_pub,?"DISARMED?by?RC");??????????}????????????arming_state_changed?=?true;????????}?else?if?(arming_ret?==?TRANSITION_DENIED)?{?????????????????????????tune_negative(true);??????}??????????????bool?first_rc_eval?=?(_last_sp_man.timestamp?==?0)?&&?(sp_man.timestamp?>?0);??????transition_result_t?main_res?=?set_main_state_rc(&status);??????????????if?(main_res?==?TRANSITION_CHANGED?||?first_rc_eval)?{??????????tune_positive(armed.armed);??????????main_state_changed?=?true;????????}?else?if?(main_res?==?TRANSITION_DENIED)?{????????????????????mavlink_log_critical(&mavlink_log_pub,?"main?state?transition?denied");??????}??????????????if?(sp_man.kill_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????????????????????????if?(!armed.lockdown)?{??????????????mavlink_log_emergency(&mavlink_log_pub,?"MANUAL?KILL?SWITCH?ENGAGED");??????????}??????????armed.lockdown?=?true;??????}?else?if?(sp_man.kill_switch?==?manual_control_setpoint_s::SWITCH_POS_OFF)?{??????????if?(armed.lockdown)?{??????????????mavlink_log_emergency(&mavlink_log_pub,?"MANUAL?KILL?SWITCH?OFF");??????????}??????????armed.lockdown?=?false;??????}??????????}?else?{??????if?(!status_flags.rc_input_blocked?&&?!status.rc_signal_lost)?{??????????mavlink_log_critical(&mavlink_log_pub,?"MANUAL?CONTROL?LOST?(at?t=%llums)",?hrt_absolute_time()?/?1000);??????????status.rc_signal_lost?=?true;??????????rc_signal_lost_timestamp?=?sp_man.timestamp;??????????status_changed?=?true;??????}??}??
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 plaincopy
int?new_mode?=_flight_mode_slots[sp_man.mode_slot];??
_flight_mode_slots的定義：
[cpp] view plaincopy
static?int32_t_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];??
[cpp] view plaincopy
static?const?int8_t?MODE_SLOT_MAX?=?6;??
也就是說_flight_mode_slots[]數組有6個元素，有6種模式可以選
賦值語句：
[cpp] view plaincopy
param_get(_param_fmode_1,&_flight_mode_slots[0]);??param_get(_param_fmode_2,&_flight_mode_slots[1]);??param_get(_param_fmode_3,&_flight_mode_slots[2]);??param_get(_param_fmode_4,&_flight_mode_slots[3]);??param_get(_param_fmode_5,&_flight_mode_slots[4]);??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 plaincopy
transition_result_t??main_state_transition(struct?vehicle_status_s?*status,?main_state_t?new_main_state,?uint8_t?&main_state_prev,????????????????status_flags_s?*status_flags,?struct?commander_state_s?*internal_state)??{??????transition_result_t?ret?=?TRANSITION_DENIED;????????????switch?(new_main_state)?{??????case?commander_state_s::MAIN_STATE_MANUAL:??????case?commander_state_s::MAIN_STATE_ACRO:??????case?commander_state_s::MAIN_STATE_RATTITUDE:??????case?commander_state_s::MAIN_STATE_STAB:??????????ret?=?TRANSITION_CHANGED;??????????break;??????case?commander_state_s::MAIN_STATE_ALTCTL:??????????????????????????????if?(!status->is_rotary_wing?||??????????????(status_flags->condition_local_altitude_valid?||???????????????status_flags->condition_global_position_valid))?{??????????????ret?=?TRANSITION_CHANGED;??????????}??????????break;??????case?commander_state_s::MAIN_STATE_POSCTL:????????????????????if?(status_flags->condition_local_position_valid?||??????????????status_flags->condition_global_position_valid)?{??????????????ret?=?TRANSITION_CHANGED;??????????}??????????break;??????case?commander_state_s::MAIN_STATE_AUTO_LOITER:????????????????????if?(status_flags->condition_global_position_valid)?{??????????????ret?=?TRANSITION_CHANGED;??????????}??????????break;??????case?commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:??????case?commander_state_s::MAIN_STATE_AUTO_MISSION:??????case?commander_state_s::MAIN_STATE_AUTO_RTL:??????case?commander_state_s::MAIN_STATE_AUTO_TAKEOFF:??????case?commander_state_s::MAIN_STATE_AUTO_LAND:????????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????ret?=?TRANSITION_CHANGED;??????????}??????????break;??????case?commander_state_s::MAIN_STATE_OFFBOARD:????????????????????if?(!status_flags->offboard_control_signal_lost)?{??????????????ret?=?TRANSITION_CHANGED;??????????}??????????break;??????case?commander_state_s::MAIN_STATE_MAX:??????default:??????????break;??????}??????if?(ret?==?TRANSITION_CHANGED)?{??????????if?(internal_state->main_state?!=?new_main_state)?{??????????????main_state_prev?=?internal_state->main_state;??????????????internal_state->main_state?=?new_main_state;??????????}?else?{??????????????ret?=?TRANSITION_NOT_CHANGED;??????????}??????}??????return?ret;??}??
[cpp] view plaincopy
transition_result_t??set_main_state_rc(struct?vehicle_status_s?*status_local)??{????????????transition_result_t?res?=?TRANSITION_DENIED;??????????????????????????if?(status_flags.offboard_control_set_by_command)?{??????????return?main_state_transition(status_local,?commander_state_s::MAIN_STATE_OFFBOARD,?main_state_prev,?&status_flags,?&internal_state);??????}??????????????if?(((_last_sp_man.timestamp?!=?0)?&&?(_last_sp_man.timestamp?==?sp_man.timestamp))?||??????????((_last_sp_man.offboard_switch?==?sp_man.offboard_switch)?&&???????????(_last_sp_man.return_switch?==?sp_man.return_switch)?&&???????????(_last_sp_man.mode_switch?==?sp_man.mode_switch)?&&???????????(_last_sp_man.acro_switch?==?sp_man.acro_switch)?&&???????????(_last_sp_man.rattitude_switch?==?sp_man.rattitude_switch)?&&???????????(_last_sp_man.posctl_switch?==?sp_man.posctl_switch)?&&???????????(_last_sp_man.loiter_switch?==?sp_man.loiter_switch)?&&???????????(_last_sp_man.mode_slot?==?sp_man.mode_slot)))?{????????????????????????if?(!rtl_on)?{??????????????_last_sp_man.timestamp?=?sp_man.timestamp;??????????????_last_sp_man.x?=?sp_man.x;??????????????_last_sp_man.y?=?sp_man.y;??????????????_last_sp_man.z?=?sp_man.z;??????????????_last_sp_man.r?=?sp_man.r;??????????}??????????????????????return?TRANSITION_NOT_CHANGED;??????}????????_last_sp_man?=?sp_man;??????????????if?(sp_man.offboard_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_OFFBOARD,?main_state_prev,?&status_flags,?&internal_state);????????????if?(res?==?TRANSITION_DENIED)?{??????????????print_reject_mode(status_local,?"OFFBOARD");??????????????????????????}?else?{????????????????????????????return?res;??????????}??????}??????????????if?(sp_man.return_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????warnx("RTL?switch?changed?and?ON!");??????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_RTL,?main_state_prev,?&status_flags,?&internal_state);????????????if?(res?==?TRANSITION_DENIED)?{??????????????print_reject_mode(status_local,?"AUTO?RTL");??????????????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_LOITER,?main_state_prev,?&status_flags,?&internal_state);??????????}????????????if?(res?!=?TRANSITION_DENIED)?{????????????????????????????return?res;??????????}??????????????????}??????????????if?(sp_man.mode_slot?!=?manual_control_setpoint_s::MODE_SLOT_NONE)?{????????????if?(sp_man.mode_slot?>=?sizeof(_flight_mode_slots)?/?sizeof(_flight_mode_slots[0]))?{??????????????warnx("m?slot?overflow");??????????????return?TRANSITION_DENIED;??????????}????????????int?new_mode?=?_flight_mode_slots[sp_man.mode_slot];????????????if?(new_mode?<?0)?{????????????????????????????res?=?TRANSITION_NOT_CHANGED;????????????}?else?{??????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);??????????????????????????????int?maxcount?=?5;????????????????????????????????????????????while?(res?==?TRANSITION_DENIED?&&?maxcount?>?0)?{????????????????????maxcount--;????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_MISSION)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_AUTO_LOITER;??????????????????????print_reject_mode(status_local,?"AUTO?MISSION");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_RTL)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_AUTO_LOITER;??????????????????????print_reject_mode(status_local,?"AUTO?RTL");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_LAND)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_AUTO_LOITER;??????????????????????print_reject_mode(status_local,?"AUTO?LAND");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_TAKEOFF)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_AUTO_LOITER;??????????????????????print_reject_mode(status_local,?"AUTO?TAKEOFF");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_AUTO_LOITER;??????????????????????print_reject_mode(status_local,?"AUTO?FOLLOW");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_LOITER)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_POSCTL;??????????????????????print_reject_mode(status_local,?"AUTO?HOLD");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_POSCTL)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_ALTCTL;??????????????????????print_reject_mode(status_local,?"POSITION?CONTROL");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_ALTCTL)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_STAB;??????????????????????print_reject_mode(status_local,?"ALTITUDE?CONTROL");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}????????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_STAB)?{??????????????????????????????????????????????new_mode?=?commander_state_s::MAIN_STATE_MANUAL;??????????????????????print_reject_mode(status_local,?"STABILIZED");??????????????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);????????????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????????????break;??????????????????????}??????????????????}??????????????}??????????}????????????return?res;??????}??????????????switch?(sp_man.mode_switch)?{??????case?manual_control_setpoint_s::SWITCH_POS_NONE:??????????res?=?TRANSITION_NOT_CHANGED;??????????break;????????case?manual_control_setpoint_s::SWITCH_POS_OFF:???????????????if?(sp_man.acro_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????????????????????????????????????????if?(status.is_rotary_wing?&&?!status.is_vtol)?{??????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_ACRO,?main_state_prev,?&status_flags,?&internal_state);??????????????}?else?if?(!status.is_rotary_wing)?{??????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_STAB,?main_state_prev,?&status_flags,?&internal_state);??????????????}?else?{??????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);??????????????}????????????}??????????else?if(sp_man.rattitude_switch?==?manual_control_setpoint_s::SWITCH_POS_ON){?????????????????????????????if?(status.is_rotary_wing)?{??????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_RATTITUDE,?main_state_prev,?&status_flags,?&internal_state);??????????????}?else?{??????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_STAB,?main_state_prev,?&status_flags,?&internal_state);??????????????}??????????}else?{??????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);??????????}??????????????????????break;????????case?manual_control_setpoint_s::SWITCH_POS_MIDDLE:????????????????if?(sp_man.posctl_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_POSCTL,?main_state_prev,?&status_flags,?&internal_state);????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????break;????????????????}????????????????print_reject_mode(status_local,?"POSITION?CONTROL");??????????}??????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_ALTCTL,?main_state_prev,?&status_flags,?&internal_state);????????????if?(res?!=?TRANSITION_DENIED)?{??????????????break;????????????}????????????if?(sp_man.posctl_switch?!=?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????????print_reject_mode(status_local,?"ALTITUDE?CONTROL");??????????}??????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);????????????????????break;????????case?manual_control_setpoint_s::SWITCH_POS_ON:????????????????????if?(sp_man.loiter_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_LOITER,?main_state_prev,?&status_flags,?&internal_state);????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????break;????????????????}????????????????print_reject_mode(status_local,?"AUTO?PAUSE");????????????}?else?{??????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_MISSION,?main_state_prev,?&status_flags,?&internal_state);????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????break;????????????????}????????????????print_reject_mode(status_local,?"AUTO?MISSION");??????????????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_LOITER,?main_state_prev,?&status_flags,?&internal_state);????????????????if?(res?!=?TRANSITION_DENIED)?{??????????????????break;????????????????}??????????}??????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_POSCTL,?main_state_prev,?&status_flags,?&internal_state);????????????if?(res?!=?TRANSITION_DENIED)?{??????????????break;????????????}??????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_ALTCTL,?main_state_prev,?&status_flags,?&internal_state);????????????if?(res?!=?TRANSITION_DENIED)?{??????????????break;????????????}??????????????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);????????????????????break;????????default:??????????break;??????}????????return?res;??}??
3.set_nav_state();根據internal_state->main_state和飛行器狀態status_flags(傳感器等硬件正常否)確定能否完成internal_state->main_state所指定的模式，若飛行器狀態不行，則將模式跟新為status->nav_state。
internal_state->main_state包含下面14種：
[cpp] view plaincopy
#define?MAIN_STATE_MANUAL?0??#define?MAIN_STATE_ALTCTL?1??#define?MAIN_STATE_POSCTL?2??#define?MAIN_STATE_AUTO_MISSION?3??#define?MAIN_STATE_AUTO_LOITER?4??#define?MAIN_STATE_AUTO_RTL?5??#define?MAIN_STATE_ACRO?6??#define?MAIN_STATE_OFFBOARD?7??#define?MAIN_STATE_STAB?8??#define?MAIN_STATE_RATTITUDE?9??#define?MAIN_STATE_AUTO_TAKEOFF?10??#define?MAIN_STATE_AUTO_LAND?11??#define?MAIN_STATE_AUTO_FOLLOW_TARGET?12??#define?MAIN_STATE_MAX?13???
[cpp] view plaincopy
<pre?name="code"?class="cpp">command.cpp中main函數??
[cpp] view plaincopy
<pre?name="code"?class="cpp">??bool?nav_state_changed?=?set_nav_state(&status,?????????????????????????&internal_state,?????????????????????????(datalink_loss_enabled?>?0),?????????????????????????mission_result.finished,?????????????????????????mission_result.stay_in_failsafe,?????????????????????????&status_flags,?????????????????????????land_detector.landed,?????????????????????????(rc_loss_enabled?>?0));??
[cpp] view plaincopy
????bool?set_nav_state(struct?vehicle_status_s?*status,?struct?commander_state_s?*internal_state,?????????????const?bool?data_link_loss_enabled,?const?bool?mission_finished,?????????????const?bool?stay_in_failsafe,?status_flags_s?*status_flags,?bool?landed,?const?bool?rc_loss_enabled)??{??????navigation_state_t?nav_state_old?=?status->nav_state;????????bool?armed?=?(status->arming_state?==?vehicle_status_s::ARMING_STATE_ARMED?||?status->arming_state?==?vehicle_status_s::ARMING_STATE_ARMED_ERROR);??????status->failsafe?=?false;??????????????switch?(internal_state->main_state)?{??????case?commander_state_s::MAIN_STATE_ACRO:??????case?commander_state_s::MAIN_STATE_MANUAL:??????case?commander_state_s::MAIN_STATE_RATTITUDE:??????case?commander_state_s::MAIN_STATE_STAB:??????case?commander_state_s::MAIN_STATE_ALTCTL:??????case?commander_state_s::MAIN_STATE_POSCTL:????????????????????if?(rc_loss_enabled?&&?(status->rc_signal_lost?||?status_flags->rc_signal_lost_cmd)?&&?armed?&&?!landed)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;??????????????}?else?if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}????????????}?else?{??????????????switch?(internal_state->main_state)?{??????????????case?commander_state_s::MAIN_STATE_ACRO:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_ACRO;??????????????????break;????????????????case?commander_state_s::MAIN_STATE_MANUAL:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_MANUAL;??????????????????break;????????????????case?commander_state_s::MAIN_STATE_RATTITUDE:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_RATTITUDE;??????????????????break;????????????????case?commander_state_s::MAIN_STATE_STAB:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_STAB;??????????????????break;????????????????case?commander_state_s::MAIN_STATE_ALTCTL:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_ALTCTL;??????????????????break;????????????????case?commander_state_s::MAIN_STATE_POSCTL:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_POSCTL;??????????????????break;????????????????default:??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_MANUAL;??????????????????break;??????????????}??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_MISSION:??????????????????????????????????????if?(status->engine_failure_cmd)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?(status_flags->data_link_lost_cmd)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;??????????}?else?if?(status_flags->gps_failure_cmd)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;??????????}?else?if?(status_flags->rc_signal_lost_cmd)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;??????????}?else?if?(status_flags->vtol_transition_failure_cmd)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;??????????????????????}?else?if?(status_flags->gps_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;??????????}?else?if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?(status_flags->vtol_transition_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;??????????}?else?if?(status->mission_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;???????????????????????}?else?if?(data_link_loss_enabled?&&?status->data_link_lost)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;??????????????}?else?if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}????????????????????????}?else?if?(!data_link_loss_enabled?&&?((status->rc_signal_lost?&&?status->data_link_lost)?||?????????????????????????????????(status->rc_signal_lost?&&?mission_finished)))?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;??????????????}?else?if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????????????????}?else?if?(!stay_in_failsafe){??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_LOITER:????????????????????if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;????????????????????}?else?if?(status->data_link_lost?&&?data_link_loss_enabled)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;??????????????}?else?if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????????????????}?else?if?(status->rc_signal_lost?&&?!data_link_loss_enabled)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;??????????????}?else?if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????????????????}?else?if?(status->rc_signal_lost)?{??????????????????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;??????????}?else?{????????????????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_RTL:??????????????????????if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?((!status_flags->condition_global_position_valid?||??????????????????????!status_flags->condition_home_position_valid))?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????}?else?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:??????????????????????if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?(!status_flags->condition_global_position_valid)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????}?else?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_TAKEOFF:??????????????????????if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?((!status_flags->condition_global_position_valid?||??????????????????????!status_flags->condition_home_position_valid))?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????}?else?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_AUTO_LAND:??????????????????????if?(status->engine_failure)?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??????????}?else?if?((!status_flags->condition_global_position_valid?||??????????????????????!status_flags->condition_home_position_valid))?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????}?else?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????}??????????break;????????case?commander_state_s::MAIN_STATE_OFFBOARD:????????????????????if?(status_flags->offboard_control_signal_lost?&&?!status->rc_signal_lost)?{??????????????status->failsafe?=?true;????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_POSCTL;??????????}?else?if?(status_flags->offboard_control_signal_lost?&&?status->rc_signal_lost)?{??????????????status->failsafe?=?true;????????????????if?(status_flags->condition_local_position_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;??????????????}?else?if?(status_flags->condition_local_altitude_valid)?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_DESCEND;??????????????}?else?{??????????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_TERMINATION;??????????????}??????????}?else?{??????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_OFFBOARD;??????????}??????default:??????????break;??????}????????return?status->nav_state?!=?nav_state_old;??}??
4.set_control_mode();根據status.nav_state確定control_mode.flag_xxx
command.cpp的main函數中
[cpp] view plaincopy
??????if?(counter?%?(200000?/?COMMANDER_MONITORING_INTERVAL)?==?0?||?status_changed)?{??????set_control_mode();??????control_mode.timestamp?=?now;??????orb_publish(ORB_ID(vehicle_control_mode),?control_mode_pub,?&control_mode);??????status.timestamp?=?now;??????orb_publish(ORB_ID(vehicle_status),?status_pub,?&status);??????armed.timestamp?=?now;????????????if?(safety.safety_switch_available)?{????????????????????armed.prearmed?=?safety.safety_off;??????????}?else?{???????????????????????armed.prearmed?=?(hrt_elapsed_time(&commander_boot_timestamp)?>?5?*?1000?*?1000);??????}??????orb_publish(ORB_ID(actuator_armed),?armed_pub,?&armed);??}??
status.nav_state可分為
[cpp] view plaincopy
static?const?uint8_t?NAVIGATION_STATE_MANUAL?=?0;??static?const?uint8_t?NAVIGATION_STATE_ALTCTL?=?1;??static?const?uint8_t?NAVIGATION_STATE_POSCTL?=?2;??static?const?uint8_t?NAVIGATION_STATE_AUTO_MISSION?=?3;??static?const?uint8_t?NAVIGATION_STATE_AUTO_LOITER?=?4;??static?const?uint8_t?NAVIGATION_STATE_AUTO_RTL?=?5;??static?const?uint8_t?NAVIGATION_STATE_AUTO_RCRECOVER?=?6;??static?const?uint8_t?NAVIGATION_STATE_AUTO_RTGS?=?7;??static?const?uint8_t?NAVIGATION_STATE_AUTO_LANDENGFAIL?=?8;??static?const?uint8_t?NAVIGATION_STATE_AUTO_LANDGPSFAIL?=?9;??static?const?uint8_t?NAVIGATION_STATE_ACRO?=?10;??static?const?uint8_t?NAVIGATION_STATE_UNUSED?=?11;??static?const?uint8_t?NAVIGATION_STATE_DESCEND?=?12;??static?const?uint8_t?NAVIGATION_STATE_TERMINATION?=?13;??static?const?uint8_t?NAVIGATION_STATE_OFFBOARD?=?14;??序只寫到了這??static?const?uint8_t?NAVIGATION_STATE_STAB?=?15;??static?const?uint8_t?NAVIGATION_STATE_RATTITUDE?=?16;??static?const?uint8_t?NAVIGATION_STATE_AUTO_TAKEOFF?=?17;??static?const?uint8_t?NAVIGATION_STATE_AUTO_LAND?=?18;??static?const?uint8_t?NAVIGATION_STATE_AUTO_FOLLOW_TARGET?=?19;??static?const?uint8_t?NAVIGATION_STATE_MAX?=?20;??
需要確定的有
[cpp] view plaincopy
control_mode.flag_control_manual_enabled?=?true;??control_mode.flag_control_auto_enabled?=?false;??control_mode.flag_control_rates_enabled?=?stabilization_required();??control_mode.flag_control_attitude_enabled?=?stabilization_required();??control_mode.flag_control_rattitude_enabled?=?false;??control_mode.flag_control_altitude_enabled?=?false;??control_mode.flag_control_climb_rate_enabled?=?false;??control_mode.flag_control_position_enabled?=?false;??control_mode.flag_control_velocity_enabled?=?false;??control_mode.flag_control_acceleration_enabled?=?false;??control_mode.flag_control_termination_enabled?=?false;??
[cpp] view plaincopy
set_control_mode()??{????????????control_mode.flag_armed?=?armed.armed;??????control_mode.flag_external_manual_override_ok?=?(!status.is_rotary_wing?&&?!status.is_vtol);??????control_mode.flag_system_hil_enabled?=?status.hil_state?==?vehicle_status_s::HIL_STATE_ON;??????control_mode.flag_control_offboard_enabled?=?false;????????switch?(status.nav_state)?{??????case?vehicle_status_s::NAVIGATION_STATE_MANUAL:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_rates_enabled?=?stabilization_required();??????????control_mode.flag_control_attitude_enabled?=?stabilization_required();??????????control_mode.flag_control_rattitude_enabled?=?false;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?false;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_STAB:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_rates_enabled?=?true;??????????control_mode.flag_control_attitude_enabled?=?true;??????????control_mode.flag_control_rattitude_enabled?=?true;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?false;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;????????????????????control_mode.flag_external_manual_override_ok?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_RATTITUDE:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_rates_enabled?=?true;??????????control_mode.flag_control_attitude_enabled?=?true;??????????control_mode.flag_control_rattitude_enabled?=?true;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?false;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_ALTCTL:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????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?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_POSCTL:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????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;????????case?vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:????????????????????control_mode.flag_external_manual_override_ok?=?false;????????????????case?vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:??????case?vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:??????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;????????case?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:??????????control_mode.flag_control_manual_enabled?=?false;??????????control_mode.flag_control_auto_enabled?=?false;??????????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?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?true;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_ACRO:??????????control_mode.flag_control_manual_enabled?=?true;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_rates_enabled?=?true;??????????control_mode.flag_control_attitude_enabled?=?false;??????????control_mode.flag_control_rattitude_enabled?=?false;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?false;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_DESCEND:????????????????????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_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?true;??????????control_mode.flag_control_termination_enabled?=?false;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_TERMINATION:????????????????????control_mode.flag_control_manual_enabled?=?false;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_rates_enabled?=?false;??????????control_mode.flag_control_attitude_enabled?=?false;??????????control_mode.flag_control_rattitude_enabled?=?false;??????????control_mode.flag_control_position_enabled?=?false;??????????control_mode.flag_control_velocity_enabled?=?false;??????????control_mode.flag_control_acceleration_enabled?=?false;??????????control_mode.flag_control_altitude_enabled?=?false;??????????control_mode.flag_control_climb_rate_enabled?=?false;??????????control_mode.flag_control_termination_enabled?=?true;??????????break;????????case?vehicle_status_s::NAVIGATION_STATE_OFFBOARD:??????????control_mode.flag_control_manual_enabled?=?false;??????????control_mode.flag_control_auto_enabled?=?false;??????????control_mode.flag_control_offboard_enabled?=?true;?????????????????????????control_mode.flag_control_rates_enabled?=?!offboard_control_mode.ignore_bodyrate?||??????????????!offboard_control_mode.ignore_attitude?||??????????????!offboard_control_mode.ignore_position?||??????????????!offboard_control_mode.ignore_velocity?||??????????????!offboard_control_mode.ignore_acceleration_force;????????????control_mode.flag_control_attitude_enabled?=?!offboard_control_mode.ignore_attitude?||??????????????!offboard_control_mode.ignore_position?||??????????????!offboard_control_mode.ignore_velocity?||??????????????!offboard_control_mode.ignore_acceleration_force;????????????control_mode.flag_control_rattitude_enabled?=?false;????????????control_mode.flag_control_acceleration_enabled?=?!offboard_control_mode.ignore_acceleration_force?&&????????????!status.in_transition_mode;????????????control_mode.flag_control_velocity_enabled?=?(!offboard_control_mode.ignore_velocity?||??????????????!offboard_control_mode.ignore_position)?&&?!status.in_transition_mode?&&??????????????!control_mode.flag_control_acceleration_enabled;????????????control_mode.flag_control_climb_rate_enabled?=?(!offboard_control_mode.ignore_velocity?||??????????????!offboard_control_mode.ignore_position)?&&?!control_mode.flag_control_acceleration_enabled;????????????control_mode.flag_control_position_enabled?=?!offboard_control_mode.ignore_position?&&?!status.in_transition_mode?&&????????????!control_mode.flag_control_acceleration_enabled;????????????control_mode.flag_control_altitude_enabled?=?(!offboard_control_mode.ignore_velocity?||??????????????!offboard_control_mode.ignore_position)?&&?!control_mode.flag_control_acceleration_enabled;????????????break;????????default:??????????break;??????}??}??
問題是不能切光流定點模式，按以上流程分析：
main_state_transition();里面
[cpp] view plaincopy
case?commander_state_s::MAIN_STATE_POSCTL:????????????if?(status_flags->condition_local_position_valid?||??????????status_flags->condition_global_position_valid)?{??????????ret?=?TRANSITION_CHANGED;??????}??????break;??
要想切換模式status_flags->condition_local_position_valid或者status_flags->condition_global_position_valid要為1
[cpp] view plaincopy
??orb_check(global_position_sub,?&updated);??if?(updated)?{????????????vehicle_global_position_s?gpos;??????orb_copy(ORB_ID(vehicle_global_position),?global_position_sub,?&gpos);????????????????????????if?(status_flags.condition_global_position_valid)?{??????????if?(gpos.eph?<?eph_threshold?*?2.5f)?{??????????????orb_copy(ORB_ID(vehicle_global_position),?global_position_sub,?&global_position);??????????}??????}?else?{??????????if?(gpos.eph?<?eph_threshold)?{??????????????orb_copy(ORB_ID(vehicle_global_position),?global_position_sub,?&global_position);??????????}??????}??}????orb_check(local_position_sub,?&updated);??if?(updated)?{????????????orb_copy(ORB_ID(vehicle_local_position),?local_position_sub,?&local_position);??}????orb_check(attitude_sub,?&updated);??if?(updated)?{????????????orb_copy(ORB_ID(vehicle_attitude),?attitude_sub,?&attitude);??}??????if?(hrt_absolute_time()?-?global_position.timestamp?>?POSITION_TIMEOUT)?{????????????if?(status_flags.condition_global_position_valid)?{??????????set_tune_override(TONE_GPS_WARNING_TUNE);??????????status_changed?=?true;??????????status_flags.condition_global_position_valid?=?false;??????}??}?else?if?(global_position.timestamp?!=?0)?{????????????if?(!status_flags.condition_global_position_valid)?{??????????status_changed?=?true;??????????status_flags.condition_global_position_valid?=?true;??????}??}??????bool?local_eph_good;??if?(status_flags.condition_local_position_valid)?{??????if?(local_position.eph?>?eph_threshold?*?2.5f)?{??????????local_eph_good?=?false;??????}?else?{??????????local_eph_good?=?true;??????}??}?else?{??????if?(local_position.eph?<?eph_threshold)?{??????????local_eph_good?=?true;??????}?else?{??????????local_eph_good?=?false;??????}??}??check_valid(local_position.timestamp,?POSITION_TIMEOUT,?local_position.xy_valid??????????&&?local_eph_good,?&(status_flags.condition_local_position_valid),?&status_changed);??check_valid(local_position.timestamp,?POSITION_TIMEOUT,?local_position.z_valid,??????????&(status_flags.condition_local_altitude_valid),?&status_changed);??
其中void check_valid()原函數為
[cpp] view plaincopy
check_valid(hrt_abstime?timestamp,?hrt_abstime?timeout,?bool?valid_in,?bool?*valid_out,?bool?*changed)??{??????hrt_abstime?t?=?hrt_absolute_time();??????bool?valid_new?=?(t?<?timestamp?+?timeout?&&?t?>?timeout?&&?valid_in);??????if?(*valid_out?!=?valid_new)?{??????????*valid_out?=?valid_new;??????????*changed?=?true;??????}??}??
由此可知
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)來自位置估計
總結
以上是生活随笔為你收集整理的pixhawk commander.cpp的飞行模式切换解读的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
