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)
- /* 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的主程序中
- /* 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
- int new_mode =_flight_mode_slots[sp_man.mode_slot];
_flight_mode_slots的定义:
- static int32_t_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
- static const int8_t MODE_SLOT_MAX = 6;
也就是说_flight_mode_slots[]数组有6个元素,有6种模式可以选
赋值语句:
- 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
- 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;
- }
- 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种:
- #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
- <pre name="code" class="cpp">command.cpp中main函数
- <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));
- /**
- * 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函数中
- /* 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可分为
- 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;
需要确定的有
- 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;
- 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();里面
- 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
- /* 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()原函数为
- 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)来自位置估计