pixhawk commander.cpp的飞行模式切换解读
commander.cpp逻辑性太强了，涉及整个系统的运作，所以分别拆分成小块看
另此篇blog大部分是参考（Pixhawk原生固件解读）飞行模式，控制模式的思路，笔者重新整理一下
此部分探究是因为进入不了光流定点模式，于是查看commander.cpp飞行模式切换部分
流程是：
(1)sensors.cpp发布ORB_ID(manual_control_setpoint)
(2)commander.cpp里set_main_state_rc()函数里的main_state_transition()函数根据遥控信息和飞行器状态status_flags决定是否能更变internal_state->main_state
(3)commander.cpp里set_nav_state()函数根据internal_state->main_state和飞行器状态status_flags(传感器等硬件正常否)确定能否完成internal_state->main_state所指定的模式，若飞行器状态不行，则将模式跟新为status->nav_state
(4)commander.cpp里set_control_mode()函数根据status.nav_state确定control_mode.flag_xxx
1. 遥控器端
Firmware/src/modules/sensors/sensors.cpp发布ORB_ID(manual_control_setpoint)
/* 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)来自位置估计