程序员经验分享-hwhale

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

2023-05-16

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

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

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

流程是：

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

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

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

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


1. 遥控器端

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

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

commander的主程序中

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

2.set_main_state_rc();函数内

2.1

orb_check(sp_man_sub, &updated);

if (updated) {

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

}

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

2.2

[cpp] view plain copy
  1. int new_mode =_flight_mode_slots[sp_man.mode_slot];  

_flight_mode_slots的定义：

[cpp] view plain copy
  1. static int32_t_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];  
[cpp] view plain copy
  1. static const int8_t MODE_SLOT_MAX = 6;  

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

赋值语句：

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

来源是用户上位机配置

mode_slot的定义：

int8_t mode_slot;

mode_slot的赋值：

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

2.3

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

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

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

internal_state->main_state包含下面14种：

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

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

command.cpp的main函数中

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

status.nav_state可分为

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

需要确定的有

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

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

main_state_transition();里面

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

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

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

其中void check_valid()原函数为

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

由此可知

status_flags.condition_global_position_valid和POSITION_TIMEOUT,global_position.timestamp有关

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

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

ORB_ID(vehicle_global_position)来自位置估计


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

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

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

ORB_ID(vehicle_local_position)来自位置估计

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

PIXHAWK

Commander

cpp

飞行模式切换解读

pixhawk commander.cpp的飞行模式切换解读 的相关文章

  • windows上使用cmake 编译yaml-cpp源码，生成yam-cpp.lib

    1 打开cmake gui 2 添加CmakeList 3 建立build 4 进入工程中生成debug和release版本的lib

  • pixhawk接口图以及引脚说明

    pixhawk接口图和引脚如下 pixhawk可以同时使用2个GPS xff0c 这只能使用其中一个罗盘 xff0c 实际中飞控挑选其中信号好的一个GPS进行定位 硬件部分 xff1a 第二个GPS插在serial 4 5接口上 xff0c

  • [pixhawk笔记]2-飞行模式

    本文翻译自px4官方开发文档 xff1a https dev px4 io en concept flight modes html xff0c 有不对之处 xff0c 敬请指正 pixhawk的飞行模式如下 xff1a MANUAL xf

  • Pixhawk串口名称与硬件接口对应关系

    Pixhawk提供的串口较多 xff0c 通过ls dev 可以看到有如下7个tty设备 xff1a ttyACM0 ttyS0 ttyS1 ttyS2 ttyS3 ttyS4 ttyS5 ttyS6 但每个串口名称对应到Pixhawk硬件

  • win7下 pixhawk (ardupilot) 的编译

    前几天都在搞pixhawk源码编译问题 xff0c 什么在window下用Console或者eclipse xff0c 还是在Ubuntu下 xff0c 都做了 xff0c 而且把 mk文件都看了 xff0c 结果还是有bug 总结一下三种

  • [C++]-yml库yaml-cpp简介

    文章目录 YAML基本语法数据类型对象数组标量引用 yaml cpp库生成器Emitter节点Node数组对象创建解析 yaml cpp是一个yml操作库 YAML YAML YAML Ain t a Markup Language xff

  • 步骤五：PIXHAWK遥控器的使用

    采用福斯i6s遥控 1 连接飞控 打开遥控器 xff0c 接收机插上飞控 xff0c 再插上送的短接线 xff0c 进行匹配对码RX 2 遥控器长按两秒锁 xff0c system output mode Output mode按照图片这样

  • sgi_stl源码学习，解析set、map背后的_Rb_tree源码(未完待续)

    参考资料 chatGPT先推荐的 算法导论 第13章 不过我手头没有这本书 https www cnblogs com skywang12345 p 3245399 html chatGPT推荐的 外加sgi stl源码 个人觉得通过源码理

  • leetcode 2. 两数相加

    2023 9 14 这道题还是有点难度 需要维护一个进位值 构造一个虚拟头节点dummy 用于结果的返回 还要构造一个当前节点cur 用于遍历修改新链表 整体思路就是长度短的链表需要补0 然后两链表从头开始遍历相加 要考虑进位 需要注意的点

  • [填坑]QT信号与槽机制注意事项

    1 信号与槽机制与回调函数性能对比 信号与槽机制比回调函数的方式要慢 当槽函数是非虚函数时 信号与槽机制大约比回到函数机制慢10倍 但依旧能够满足大多数应用的需求 因为1秒钟可以出发200万次这样的信号 i586 500机器 1个信号绑定一

  • 如何识别C++编译以后的函数名

    C C 语言在编译以后 函数的名字会被编译器修改 改成编译器内部的名字 这个名字会在链接的时候用到 如果用backtrace之类的函数打印堆栈时 显示的就是被编译器修改过的名字 比如说 Z3foov 那么这个函数真实的名字是什么呢 每个编译

  • cppcheck linux安装和使用

    环境 centos7 下载cppcheck地址 官网 ccpcheck版本 cppcheck 2 6 上传到响应的目录执行编译 unzip cppcheck 2 6 zip cd cppcheck 2 6 make 代码检查命令 cppch

  • 《算法导论》总结与分析

    算法导论总结与分析 分治 strassen算法 介绍 步骤 正确性证明 复杂度分析 排序 堆排序 介绍 步骤 构建 排序 优先队列 复杂度分析 快速排序 介绍 步骤 复杂度分析 最坏情况 最好情况 线性时间排序 介绍 步骤 复杂度分析 数据

  • leetcode 10. 正则表达式匹配

    2023 9 20 感觉是目前做过dp题里最难的一题了 本题首要的就是需要理解题意 翻了评论区我才发现之前一直理解的题意是错的 我原来理解的 匹配0次 是指 直接消失 不会影响到前面的字符 但是 和前一个字符其实是连体的 所以说 如果匹配0

  • 侯捷-C++面向对象高级开发(上)-complex类实现

    complex类实现 comlex h ifndef COMPLEX H define COMPLEX H include

  • extern C 在c/c++中的使用

    http blog csdn net jscese article details 37821961 1 问题定义 在研究操作系统源代码或者在嵌入式系统中编写程序时 经常会发现下面这种用法 cpp view plain copy print

  • 详解c++STL—容器deque

    目录 1 deque容器的基本概念 1 1 功能 1 2 deque与vector区别 1 3 deque内部工作原理 2 deque构造函数 2 1 功能描述 2 2 函数原型 2 3 示例 3 deque赋值操作 3 1 功能描述 3

  • 理解cpp的重载，重写，重定义

    函数重载 overload 函数重载是指在一个类中声明多个名称相同但参数列表不同的函数 这些的参数可能个数或顺序 类型不同 但是不能靠返回类型来判断 特征是 1 相同的范围 在同一个作用域中 2 函数名字相同 3 参数不同 4 virtua

  • c++基础：循环练习案例展示

    1 猜数字 题目 系统随机生成一个1到100的数字 玩家进行猜测 如果猜错 提示玩家数字过大或过小 如果猜对恭喜玩家胜利 并且退出游戏 代码 include

  • QCustomPlot获取选点坐标

    QCustomPlot版本 Version 2 1 1 设置点选择模式 customPlot gt setInteractions QCP iSelectPlottables 2 绑定点击事件 connect customPlot QCus

随机推荐

  • 关于博士招生“申请-考核制”，教育部这样说！

    转载于 青塔 近日 xff0c 教育部在官网上就十三届全国人大二次会议 关于改进博士生招生 申请 考核制 的建议 进行答复 答复中称 xff1a 教育部在着力探索建立博士生招生质量第三方评价机制 xff0c 推动招生单位建立健全以自我评价为

  • Keil5.26、Keil5.27、Keil5.30下载地址

    亲测有效 xff0c 速速下载 mdk5 26下载地址 http www keil com fid vquv2wwtdy9j1w9xagw1om5eu9xbkks1e66vd1 files eval mdk526 exe mdk5 27下载

  • EEPROM和flash的区别

    之前对各种存储器一直不太清楚 xff0c 今天总结一下 存储器分为两大类 xff1a ram和rom ram就不讲了 xff0c 今天主要讨论rom rom最初不能编程 xff0c 出厂什么内容就永远什么内容 xff0c 不灵活 后来出现了

  • git merge最简洁用法

    一 开发分支 xff08 dev xff09 上的代码达到上线的标准后 xff0c 要合并到 master 分支 git checkout dev git pull git checkout master git merge dev git

  • 汇编cmp比较指令详解

    刚刚看到了cmp指令 xff0c 一开始有点晕 后来上网找了些资料 xff0c 终于看明白了 xff0c 为了方便初学者 xff0c 我就简单写下我的思路吧 高手绕过 xff0c 谢谢 xff01 cmp compare 指令进行比较两个操

  • vim中复制粘贴

    在vim中要进行复制粘贴 需要使用yy和p指令 但是会发现当我们想讲从vim外面复制的内容粘贴进文本中时 光使用p达不到效果 原因是 在vim中维护者许多的clipboard 而不是只有一个 我们从vim外部 如浏览器 复制的文本所在的cl

  • NP问题真的很难理解

    希望通过这篇文章可以不仅让计算机相关专业的人可以看懂和区分什么是P类问题什么是NP类问题 xff0c 更希望达到的效果是非专业人士比如学文科的朋友也可以有一定程度的理解 有一则程序员界的笑话 xff0c 就是有一哥们去google面试的时候

  • USB转TTL、USB转串口、USB转232的区别

    PO主作为一个没有专业背景的小白 xff0c 在初玩单片机时曾被上面的几个名词所混淆 xff0c 不过后来终于大彻大悟 xff0c 现在把自己的理解写在这里 xff0c 同样准备入门单片机的小白可以看看 xff0c 或许对你有所帮助 首先

  • STM32的时钟系统RCC详细整理

    一 综述 xff1a 1 时钟源 在 STM32 中 xff0c 一共有 5 个时钟源 xff0c 分别是 HSI HSE LSI LSE PLL HSI 是高速内部时钟 xff0c RC 振荡器 xff0c 频率为 8MHz xff1b

  • 第一章 PX4程序编译过程解析

    版权声明 xff1a 本文为博主原创文章 xff0c 未经博主允许不得转载 第一章 PX4程序编译过程解析 PX4是一款软硬件开源的项目 xff0c 目的在于学习和研究 其中也有比较好的编程习惯 xff0c 大家不妨可以学习一下国外牛人的编

  • 第二章 PX4-RCS启动文件解析

    版权声明 xff1a 本文为博主原创文章 xff0c 未经博主允许不得转载 第二章 PX4 RCS启动文件解析 RCS的启动类似于linux的shell文件 xff0c 如果不知道shell文件是什么东西可以理解成是为程序的流程框 xff0

  • pixhawk position_estimator_inav.cpp思路整理及数据流

    写在前面 xff1a 这篇blog主要参考pixhawk的高度解算算法解读 xff0c 并且加以扩展 xff0c 扩展到其他传感器 xff0c 其实里面处理好多只是记录了流程 xff0c 至于里面实际物理意义并不是很清楚 xff0c 也希望

  • git创建新分支

    1 创建本地分支 git branch 分支名 xff0c 例如 xff1a git branch 2 0 1 20120806 注 xff1a 2 0 1 20120806是分支名称 xff0c 可以随便定义 2 切换本地分支 git c

  • 第一章 PX4-Pixhawk-程序编译过程解析

    第一章 PX4程序编译过程解析 PX4是一款软硬件开源的项目 xff0c 目的在于学习和研究 其中也有比较好的编程习惯 xff0c 大家不妨可以学习一下国外牛人的编程习惯 这个项目是苏黎世联邦理工大学的一个实验室搞出来的 该方案是基于NUT

  • 详解几种飞控的姿态解算算法

    姿态解算是飞控的一个基础 重要部分 xff0c 估计出来的姿态会发布给姿态控制器 xff0c 控制飞行平稳 xff0c 是飞行稳定 的最重要保障 有关姿态解算的基础知识 xff0c 这里笔者不会细细描述 xff0c 有关这方面的资料 xff

  • 陀螺仪的数据处理

    1 陀螺仪数据校准 1 1 原理 一款飞控上的传感器是需要进行校准的 xff0c 比如这里讲的陀螺仪 目前大多数的陀螺校准其实就是去掉零点偏移量 xff0c 采集一定的数据 xff0c 求平均 xff0c 这个平均值就是零点偏移 xff0c

  • LevelDB源码分析之从Put说起

    之前分享的文章leveldb实现原理分析详细描述了LevelDB的实现原理 xff0c 本文从Put接口来看下leveldb数据写流程的源码实现 LevelDB的读写接口在类DB中定义 xff08 leveldb db h xff09 xf

  • 大神浅谈无人机飞控软件设计 系统性总结

    写在前面 深感自己对飞控软件 算法的知识点过于杂乱 xff0c 很久没有进行系统的总结了 xff0c 因此决定写几篇文章记录一些飞控开发过程的知识点 主要是针对一些软件 算法部分进行讨论 xff0c 如内容有错误 xff0c 欢迎指出 1

  • 作为资深的无人机从业者，卡尔曼滤波你不能不知道 通俗易懂的来说卡尔曼滤波

    旋翼无人机的两类主要算法 先说一个旋翼类无人机系统的算法主要有两类 xff1a 姿态检测算法 姿态控制算法 姿态控制 被控对象 xff08 即四旋翼无人机 xff09 姿态检测三个部分构成一个闭环控制系统 被控对象的模型是由其物理系统决定

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

    commander cpp逻辑性太强了 xff0c 涉及整个系统的运作 xff0c 所以分别拆分成小块看 另此篇blog大部分是参考 xff08 Pixhawk原生固件解读 xff09 飞行模式 xff0c 控制模式的思路 xff0c 笔者

热门标签