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

/*?only?publish?manual?control?if?the?signal?is?still?present?*/??

if?(!signal_lost)?{??

??

????/*?initialize?manual?setpoint?*/??

????struct?manual_control_setpoint_s?manual?=?{};??

????/*?set?mode?slot?to?unassigned?*/??

????manual.mode_slot?=?manual_control_setpoint_s::MODE_SLOT_NONE;??

????/*?set?the?timestamp?to?the?last?signal?time?*/??

????manual.timestamp?=?rc_input.timestamp_last_signal;??

??

????/*?limit?controls?*/??

????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)?{??

??

????????/*?the?number?of?valid?slots?equals?the?index?of?the?max?marker?minus?one?*/??

????????const?int?num_slots?=?manual_control_setpoint_s::MODE_SLOT_MAX;??

??

????????/*?the?half?width?of?the?range?of?a?slot?is?the?total?range?

?????????*?divided?by?the?number?of?slots,?again?divided?by?two?

?????????*/??

????????const?float?slot_width_half?=?2.0f?/?num_slots?/?2.0f;??

??

????????/*?min?is?-1,?max?is?+1,?range?is?2.?We?offset?below?min?and?max?*/??

????????const?float?slot_min?=?-1.0f?-?0.05f;??

????????const?float?slot_max?=?1.0f?+?0.05f;??

??

????????/*?the?slot?gets?mapped?by?first?normalizing?into?a?0..1?interval?using?min?

?????????*?and?max.?Then?the?right?slot?is?obtained?by?multiplying?with?the?number?of?

?????????*?slots.?And?finally?we?add?half?a?slot?width?to?ensure?that?integer?rounding?

?????????*?will?take?us?to?the?correct?final?index.?

?????????*/??

????????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;??

????????}??

????}??

??

????/*?mode?switches?*/??

????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);??

??

????/*?publish?manual_control_setpoint?topic?*/??

????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

/*?RC?input?check?*/??

if?(!status_flags.rc_input_blocked?&&?sp_man.timestamp?!=?0?&&??

????(hrt_absolute_time()?<?sp_man.timestamp?+?(uint64_t)(rc_loss_timeout?*?1e6f)))?{??

????/*?handle?the?case?where?RC?signal?was?regained?*/??

????/*?處理信號失而復得的情況?*/??

????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;??

??

????/*?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?

?????*?do?it?only?for?rotary?wings?in?manual?mode?or?fixed?wing?if?landed?*/??

????/*?檢查油門桿在左下角的位置&&在手動&&(Rattitude||AUTO_READY?mode||ASSIST?mode?and?landed，如果是，則上鎖?

????if?((status.is_rotary_wing?||?(!status.is_rotary_wing?&&?land_detector.landed))?&&?status.rc_input_mode?!=?vehicle_status_s::RC_IN_MODE_OFF?&&?

????????(status.arming_state?==?vehicle_status_s::ARMING_STATE_ARMED?||?status.arming_state?==?vehicle_status_s::ARMING_STATE_ARMED_ERROR)?&&?

????????(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_RATTITUDE?||?

????????????land_detector.landed)?&&?

????????sp_man.r?<?-STICK_ON_OFF_LIMIT?&&?sp_man.z?<?0.1f)?{?

?

????????if?(stick_off_counter?>?rc_arm_hyst)?{?

????????????/*?disarm?to?STANDBY?if?ARMED?or?to?STANDBY_ERROR?if?ARMED_ERROR?*/??

????????????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?/*?fRunPreArmChecks?*/,??

?????????????????????????????????&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;??

????}??

??

????/*?check?if?left?stick?is?in?lower?right?position?and?we're?in?MANUAL?mode?->?arm?*/??

????/*?檢查油門桿在右下角的位置&&手動模式，如果是，則解鎖?*/??

????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)?{??

??

????????????/*?we?check?outside?of?the?transition?function?here?because?the?requirement?

?????????????*?for?being?in?manual?mode?only?applies?to?manual?arming?actions.?

?????????????*?the?system?can?be?armed?in?auto?if?armed?via?the?GCS.?

?????????????*/??

??

????????????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?/*?fRunPreArmChecks?*/,??

?????????????????????????????????????&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)?{??

????????/*?

?????????*?the?arming?transition?can?be?denied?to?a?number?of?reasons:?

?????????*??-?pre-flight?check?failed?(sensors?not?ok?or?not?calibrated)?

?????????*??-?safety?not?disabled?

?????????*??-?system?not?in?manual?mode?

?????????*/??

????????tune_negative(true);??

????}??

??

????/*?evaluate?the?main?state?machine?according?to?mode?switches?*/??

????bool?first_rc_eval?=?(_last_sp_man.timestamp?==?0)?&&?(sp_man.timestamp?>?0);??

????transition_result_t?main_res?=?set_main_state_rc(&status);??

??

????/*?play?tune?on?mode?change?only?if?armed,?blink?LED?always?*/??

????if?(main_res?==?TRANSITION_CHANGED?||?first_rc_eval)?{??

????????tune_positive(armed.armed);??

????????main_state_changed?=?true;??

??

????}?else?if?(main_res?==?TRANSITION_DENIED)?{??

????????/*?DENIED?here?indicates?bug?in?the?commander?*/??

????????mavlink_log_critical(&mavlink_log_pub,?"main?state?transition?denied");??

????}??

??

????/*?check?throttle?kill?switch?*/??

????if?(sp_man.kill_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??

????????/*?set?lockdown?flag?*/??

????????/*?設置鎖定標志?*/??

????????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;??

????}??

????/*?no?else?case:?do?not?change?lockdown?flag?in?unconfigured?case?*/??

??

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

????/*?transition?may?be?denied?even?if?the?same?state?is?requested?because?conditions?may?have?changed?*/??

????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:??

????????/*?need?at?minimum?altitude?estimate?*/??

????????/*?TODO:?add?this?for?fixedwing?as?well?*/??

????????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:??

????????/*?need?at?minimum?local?position?estimate?*/??

????????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:??

????????/*?need?global?position?estimate?*/??

????????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:??

????????/*?need?global?position?and?home?position?*/??

????????if?(status_flags->condition_global_position_valid?&&?status_flags->condition_home_position_valid)?{??

????????????ret?=?TRANSITION_CHANGED;??

????????}??

????????break;??

????case?commander_state_s::MAIN_STATE_OFFBOARD:??

????????/*?need?offboard?signal?*/??

????????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)??

{??

????/*?set?main?state?according?to?RC?switches?*/??

????transition_result_t?res?=?TRANSITION_DENIED;??

??

????//?XXX?this?should?not?be?necessary?any?more,?we?should?be?able?to??

????//?just?delete?this?and?respond?to?mode?switches??

????/*?if?offboard?is?set?already?by?a?mavlink?command,?abort?*/??

????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);??

????}??

??

????/*?manual?setpoint?has?not?updated,?do?not?re-evaluate?it?*/??

????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)))?{??

??

????????//?update?these?fields?for?the?geofence?system??

??

????????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;??

????????}??

??

????????/*?no?timestamp?change?or?no?switch?change?->?nothing?changed?*/??

????????return?TRANSITION_NOT_CHANGED;??

????}??

??

????_last_sp_man?=?sp_man;??

/***********************第一個判斷***********************/??

????/*?offboard?switch?overrides?main?switch?*/??

????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");??

????????????/*?mode?rejected,?continue?to?evaluate?the?main?system?mode?*/??

??

????????}?else?{??

????????????/*?changed?successfully?or?already?in?this?state?*/??

????????????return?res;??

????????}??

????}??

/***********************第二個判斷***********************/??

????/*?RTL?switch?overrides?main?switch?*/??

????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");??

??

????????????/*?fallback?to?LOITER?if?home?position?not?set?*/??

????????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_AUTO_LOITER,?main_state_prev,?&status_flags,?&internal_state);??

????????}??

??

????????if?(res?!=?TRANSITION_DENIED)?{??

????????????/*?changed?successfully?or?already?in?this?state?*/??

????????????return?res;??

????????}??

??

????????/*?if?we?get?here?mode?was?rejected,?continue?to?evaluate?the?main?system?mode?*/??

????}??

/***********************第三個判斷***********************/??

????/*?we?know?something?has?changed?-?check?if?we?are?in?mode?slot?operation?*/??

????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)?{??

????????????/*?slot?is?unused?*/??

????????????res?=?TRANSITION_NOT_CHANGED;??

??

????????}?else?{??

????????????res?=?main_state_transition(status_local,?new_mode,?main_state_prev,?&status_flags,?&internal_state);??

??

????????????/*?ensure?that?the?mode?selection?does?not?get?stuck?here?*/??

????????????int?maxcount?=?5;??

??

????????????/*?enable?the?use?of?break?*/??

????????????/*?fallback?strategies,?give?the?user?the?closest?mode?to?what?he?wanted?*/??

????????????while?(res?==?TRANSITION_DENIED?&&?maxcount?>?0)?{??

??

????????????????maxcount--;??

??

????????????????if?(new_mode?==?commander_state_s::MAIN_STATE_AUTO_MISSION)?{??

??

????????????????????/*?fall?back?to?loiter?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?position?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?position?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?position?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?position?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?position?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?altitude?control?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?stabilized?*/??

????????????????????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)?{??

??

????????????????????/*?fall?back?to?manual?*/??

????????????????????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;??

????}??

/***********************第四個判斷***********************/??

????/*?offboard?and?RTL?switches?off?or?denied,?check?main?mode?switch?*/??

????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:?????//?MANUAL??

????????if?(sp_man.acro_switch?==?manual_control_setpoint_s::SWITCH_POS_ON)?{??

??

????????????/*?manual?mode?is?stabilized?already?for?multirotors,?so?switch?to?acro?

?????????????*?for?any?non-manual?mode?

?????????????*/??

????????????//?XXX:?put?ACRO?and?STAB?on?separate?switches??

????????????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){??

????????????/*?Similar?to?acro?transitions?for?multirotors.??FW?aircraft?don't?need?a?

?????????????*?rattitude?mode.*/??

????????????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);??

????????}??

??

????????//?TRANSITION_DENIED?is?not?possible?here??

????????break;??

??

????case?manual_control_setpoint_s::SWITCH_POS_MIDDLE:??????//?ASSIST??

????????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;??//?changed?successfully?or?already?in?this?state??

????????????}??

??

????????????print_reject_mode(status_local,?"POSITION?CONTROL");??

????????}??

??

????????//?fallback?to?ALTCTL??

????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_ALTCTL,?main_state_prev,?&status_flags,?&internal_state);??

??

????????if?(res?!=?TRANSITION_DENIED)?{??

????????????break;??//?changed?successfully?or?already?in?this?mode??

????????}??

??

????????if?(sp_man.posctl_switch?!=?manual_control_setpoint_s::SWITCH_POS_ON)?{??

????????????print_reject_mode(status_local,?"ALTITUDE?CONTROL");??

????????}??

??

????????//?fallback?to?MANUAL??

????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);??

????????//?TRANSITION_DENIED?is?not?possible?here??

????????break;??

??

????case?manual_control_setpoint_s::SWITCH_POS_ON:??????????//?AUTO??

????????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;??//?changed?successfully?or?already?in?this?state??

????????????}??

??

????????????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;??//?changed?successfully?or?already?in?this?state??

????????????}??

??

????????????print_reject_mode(status_local,?"AUTO?MISSION");??

??

????????????//?fallback?to?LOITER?if?home?position?not?set??

????????????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;??//?changed?successfully?or?already?in?this?state??

????????????}??

????????}??

??

????????//?fallback?to?POSCTL??

????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_POSCTL,?main_state_prev,?&status_flags,?&internal_state);??

??

????????if?(res?!=?TRANSITION_DENIED)?{??

????????????break;??//?changed?successfully?or?already?in?this?state??

????????}??

??

????????//?fallback?to?ALTCTL??

????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_ALTCTL,?main_state_prev,?&status_flags,?&internal_state);??

??

????????if?(res?!=?TRANSITION_DENIED)?{??

????????????break;??//?changed?successfully?or?already?in?this?state??

????????}??

??

????????//?fallback?to?MANUAL??

????????res?=?main_state_transition(status_local,?commander_state_s::MAIN_STATE_MANUAL,?main_state_prev,?&status_flags,?&internal_state);??

????????//?TRANSITION_DENIED?is?not?possible?here??

????????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">/*?now?set?navigation?state?according?to?failsafe?and?main?state?*/??

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

/**?

?*?Check?failsafe?and?main?status?and?set?navigation?status?for?navigator?accordingly?

?*/??

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;??

??

????/*?evaluate?main?state?to?decide?in?normal?(non-failsafe)?mode?*/??

????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:??

????????/*?require?RC?for?all?manual?modes?*/??

????????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:??

??

????????/*?go?into?failsafe?

?????????*?-?if?commanded?to?do?so?

?????????*?-?if?we?have?an?engine?failure?

?????????*?-?if?we?have?vtol?transition?failure?

?????????*?-?depending?on?datalink,?RC?and?if?the?mission?is?finished?*/??

??

????????/*?first?look?at?the?commands?*/??

????????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;??

??

????????/*?finished?handling?commands?which?have?priority,?now?handle?failures?*/??

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

??

????????/*?datalink?loss?enabled:?

?????????*?check?for?datalink?lost:?this?should?always?trigger?RTGS?*/??

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

????????????}??

??

????????/*?datalink?loss?disabled:?

?????????*?check?if?both,?RC?and?datalink?are?lost?during?the?mission?

?????????*?or?RC?is?lost?after?the?mission?is?finished:?this?should?always?trigger?RCRECOVER?*/??

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

????????????}??

??

????????/*?stay?where?you?are?if?you?should?stay?in?failsafe,?otherwise?everything?is?perfect?*/??

????????}?else?if?(!stay_in_failsafe){??

????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;??

????????}??

????????break;??

??

????case?commander_state_s::MAIN_STATE_AUTO_LOITER:??

????????/*?go?into?failsafe?on?a?engine?failure?*/??

????????if?(status->engine_failure)?{??

????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;??

????????/*?also?go?into?failsafe?if?just?datalink?is?lost?*/??

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

????????????}??

??

????????/*?go?into?failsafe?if?RC?is?lost?and?datalink?loss?is?not?set?up?*/??

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

????????????}??

??

????????/*?don't?bother?if?RC?is?lost?if?datalink?is?connected?*/??

????????}?else?if?(status->rc_signal_lost)?{??

??

????????????/*?this?mode?is?ok,?we?don't?need?RC?for?loitering?*/??

????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;??

????????}?else?{??

????????????/*?everything?is?perfect?*/??

????????????status->nav_state?=?vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;??

????????}??

????????break;??

??

????case?commander_state_s::MAIN_STATE_AUTO_RTL:??

????????/*?require?global?position?and?home,?also?go?into?failsafe?on?an?engine?failure?*/??

??

????????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:??

????????/*?require?global?position?and?home?*/??

??

????????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:??

????????/*?require?global?position?and?home?*/??

??

????????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:??

????????/*?require?global?position?and?home?*/??

??

????????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:??

????????/*?require?offboard?control,?otherwise?stay?where?you?are?*/??

????????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

/*?publish?states?(armed,?control?mode,?vehicle?status)?at?least?with?5?Hz?*/??

????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;??

????/*?set?prearmed?state?if?safety?is?off,?or?safety?is?not?present?and?5?seconds?passed?*/??

????if?(safety.safety_switch_available)?{??

????????/*?safety?is?off,?go?into?prearmed?*/??

????????armed.prearmed?=?safety.safety_off;??

????????}?else?{??

????????/*?safety?is?not?present,?go?into?prearmed?

?????????*?(all?output?drivers?should?be?started?/?unlocked?last?in?the?boot?process?

?????????*?when?the?rest?of?the?system?is?fully?initialized)?

?????????*/??

????????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()??

{??

????/*?set?vehicle_control_mode?according?to?set_navigation_state?*/??

????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;??

????????/*?override?is?not?ok?in?stabilized?mode?*/??

????????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:??

????????/*?override?is?not?ok?for?the?RTL?and?recovery?mode?*/??

????????control_mode.flag_external_manual_override_ok?=?false;??

????????/*?fallthrough?*/??

????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:??

????????/*?TODO:?check?if?this?makes?sense?*/??

????????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:??

????????/*?disable?all?controllers?on?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;??

??

????????/*?

?????????*?The?control?flags?depend?on?what?is?ignored?according?to?the?offboard?control?mode?topic?

?????????*?Inner?loop?flags?(e.g.?attitude)?also?depend?on?outer?loop?ignore?flags?(e.g.?position)?

?????????*/??

????????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:??

????/*?need?at?minimum?local?position?estimate?*/??

????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

/*?update?global?position?estimate?*/??

orb_check(global_position_sub,?&updated);??

if?(updated)?{??

????/*?position?changed?*/??

????vehicle_global_position_s?gpos;??

????orb_copy(ORB_ID(vehicle_global_position),?global_position_sub,?&gpos);??

????/*?copy?to?global?struct?if?valid,?with?hysteresis?*/??

????//?XXX?consolidate?this?with?local?position?handling?and?timeouts?after?release??

????//?but?we?want?a?low-risk?change?now.??

????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);??

????????}??

????}??

}??

/*?update?local?position?estimate?*/??

orb_check(local_position_sub,?&updated);??

if?(updated)?{??

????/*?position?changed?*/??

????orb_copy(ORB_ID(vehicle_local_position),?local_position_sub,?&local_position);??

}??

/*?update?attitude?estimate?*/??

orb_check(attitude_sub,?&updated);??

if?(updated)?{??

????/*?position?changed?*/??

????orb_copy(ORB_ID(vehicle_attitude),?attitude_sub,?&attitude);??

}??

//update?condition_global_position_valid??

//Global?positions?are?only?published?by?the?estimators?if?they?are?valid??

if?(hrt_absolute_time()?-?global_position.timestamp?>?POSITION_TIMEOUT)?{??

????//We?have?had?no?good?fix?for?POSITION_TIMEOUT?amount?of?time??

????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)?{??

????//?Got?good?global?position?estimate??

????if?(!status_flags.condition_global_position_valid)?{??

????????status_changed?=?true;??

????????status_flags.condition_global_position_valid?=?true;??

????}??

}??

/*?update?condition_local_position_valid?and?condition_local_altitude_valid?*/??

/*?hysteresis?for?EPH?*/??

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的飞行模式切换解读的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。