yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander: centralize main_state strings and simplify main state change attempts

Browse Source 
* commander: centralize main_state strings and simplify main state change attempts
This commit is contained in:
Daniel Agar
2021-03-30 08:52:36 -04:00
committed by Matthias Grob
parent 18be1bacdc
commit 0fa91f7cb0
2 changed files with 89 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
src/modules/commander/Commander.cpp
View File

@@ -435,6 +435,41 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
return "";
};
static constexpr const char *main_state_str(uint8_t main_state)
{
switch (main_state) {
case commander_state_s::MAIN_STATE_MANUAL: return "Manual";
case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude";
case commander_state_s::MAIN_STATE_POSCTL: return "Position";
case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission";
case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold";
case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL";
case commander_state_s::MAIN_STATE_ACRO: return "Acro";
case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard";
case commander_state_s::MAIN_STATE_STAB: return "Stabilized";
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff";
case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land";
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target";
case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land";
case commander_state_s::MAIN_STATE_ORBIT: return "Orbit";
default: return "Unknown";
}
}
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
@@ -513,102 +548,66 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
}
transition_result_t
Commander::try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback)
Commander::try_mode_change(main_state_t desired_mode)
{
transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
print_reject_mode(desired_mode);
if (desired_mode == commander_state_s::MAIN_STATE_OFFBOARD) {
/* offboard does not have a fallback */
print_reject_mode("Offboard");
return res;
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
print_reject_mode("Auto Mission");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_RTL && (res == TRANSITION_DENIED)) {
print_reject_mode("Auto RTL");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LAND && (res == TRANSITION_DENIED)) {
print_reject_mode("Auto Land");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && (res == TRANSITION_DENIED)) {
print_reject_mode("Auto Takeoff");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET && (res == TRANSITION_DENIED)) {
print_reject_mode("Auto Follow");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LOITER && (res == TRANSITION_DENIED)) {
print_reject_mode("Auto Hold");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
/* fall back to position control */
desired_mode = commander_state_s::MAIN_STATE_POSCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
/* fall back to position control */
desired_mode = commander_state_s::MAIN_STATE_POSCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_POSCTL && (res == TRANSITION_DENIED)) {
print_reject_mode("Position");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
/* fall back to altitude control */
desired_mode = commander_state_s::MAIN_STATE_ALTCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
/* fall back to altitude control */
desired_mode = commander_state_s::MAIN_STATE_ALTCTL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_ALTCTL && (res == TRANSITION_DENIED)) {
print_reject_mode("Altitude");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
/* fall back to stabilized */
desired_mode = commander_state_s::MAIN_STATE_STAB;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
/* fall back to stabilized */
desired_mode = commander_state_s::MAIN_STATE_STAB;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
if (desired_mode == commander_state_s::MAIN_STATE_STAB && (res == TRANSITION_DENIED)) {
print_reject_mode("Stabilized");
if (enable_fallback == FlightModeChange::FallbackEnabled) {
/* fall back to manual */
desired_mode = commander_state_s::MAIN_STATE_MANUAL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
/* fall back to manual */
desired_mode = commander_state_s::MAIN_STATE_MANUAL;
res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
}
}
@@ -2243,11 +2242,9 @@ Commander::run()
}
}
if (_manual_control.isMavlink()) {
if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) {
// if there's never been a mode change force position control as initial state
if (!_armed.armed && (_internal_state.main_state_changes == 0)) {
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
}
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
}
if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
@@ -2942,9 +2939,13 @@ Commander::set_main_state_rc()
/* offboard switch overrides main switch */
if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_OFFBOARD, FlightModeChange::FallbackDisabled);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD);
/* mode rejected, continue to evaluate the main system mode */
} else {
/* changed successfully or already in this state */
return res;
}
@@ -2952,8 +2953,7 @@ Commander::set_main_state_rc()
/* RTL switch overrides main switch */
if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL, FlightModeChange::FallbackEnabled);
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
/* changed successfully or already in this state */
@@ -2965,9 +2965,14 @@ Commander::set_main_state_rc()
/* Loiter switch overrides main switch */
if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_LOITER, FlightModeChange::FallbackDisabled);
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER);
/* mode rejected, continue to evaluate the main system mode */
} else {
/* changed successfully or already in this state */
return res;
}
}
@@ -2987,7 +2992,7 @@ Commander::set_main_state_rc()
res = TRANSITION_NOT_CHANGED;
} else {
res = try_mode_change(new_mode, FlightModeChange::FallbackEnabled);
res = try_mode_change(new_mode);
}
return res;
@@ -3053,31 +3058,16 @@ Commander::set_main_state_rc()
case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST
if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL, FlightModeChange::FallbackDisabled);
res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else {
res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL);
}
// fallback to ALTCTL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) {
print_reject_mode("Altitude");
}
// fallback to MANUAL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
// TRANSITION_DENIED is not possible here
break;
case manual_control_switches_s::SWITCH_POS_ON: // AUTO
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION, FlightModeChange::FallbackEnabled);
case manual_control_switches_s::SWITCH_POS_ON: // AUTO
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION);
break;
default:
@@ -3308,17 +3298,17 @@ Commander::stabilization_required()
}
void
Commander::print_reject_mode(const char *msg)
Commander::print_reject_mode(uint8_t main_state)
{
const hrt_abstime t = hrt_absolute_time();
if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
_last_print_mode_reject_time = t;
mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", msg);
mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", main_state_str(main_state));
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
tune_negative(_armed.armed);
_last_print_mode_reject_time = hrt_absolute_time();
}
}

11
src/modules/commander/Commander.hpp
View File

@@ -122,17 +122,11 @@ public:
void get_circuit_breaker_params();
private:
enum class FlightModeChange {
FallbackDisabled = 0,
FallbackEnabled
};
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
transition_result_t disarm(arm_disarm_reason_t calling_reason);
transition_result_t try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback);
transition_result_t try_mode_change(main_state_t desired_mode);
void battery_status_check();
@@ -161,7 +155,7 @@ private:
void offboard_control_update();
void print_reject_mode(const char *msg);
void print_reject_mode(uint8_t main_state);
void reset_posvel_validity();
@@ -292,7 +286,6 @@ private:
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms};
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */