From e635fccd2541a6e863b620f5a299d1ea0cf2cfc5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 29 Mar 2021 11:39:00 -0400 Subject: [PATCH] commander: centralize main_state strings and use for rejection message --- src/modules/commander/Commander.cpp | 104 ++++++++++++++++++---------- src/modules/commander/Commander.hpp | 2 +- 2 files changed, 67 insertions(+), 39 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 672fcbc138..d57389c797 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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 control"; + + case commander_state_s::MAIN_STATE_POSCTL: return "position control"; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: return "auto mission"; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: return "auto 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 "auto takeoff"; + + case commander_state_s::MAIN_STATE_AUTO_LAND: return "auto land"; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "follow target"; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "auto 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 @@ -2835,7 +2870,7 @@ Commander::set_main_state_rc() res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); + print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD); /* mode rejected, continue to evaluate the main system mode */ } else { @@ -2849,7 +2884,7 @@ Commander::set_main_state_rc() res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, &_internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode("AUTO RTL"); + print_reject_mode(commander_state_s::MAIN_STATE_AUTO_RTL); /* fallback to LOITER if home position not set */ res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); @@ -2868,7 +2903,7 @@ Commander::set_main_state_rc() res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode("AUTO HOLD"); + print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER); } else { return res; @@ -2902,10 +2937,9 @@ Commander::set_main_state_rc() maxcount--; if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) { - - /* fall back to loiter */ + // fall back to loiter + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO MISSION"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2914,10 +2948,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) { - - /* fall back to position control */ + // fall back to position control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO RTL"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2926,10 +2959,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) { - - /* fall back to position control */ + // fall back to position control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO LAND"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2938,10 +2970,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { - - /* fall back to position control */ + // fall back to position control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO TAKEOFF"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2950,10 +2981,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) { - - /* fall back to position control */ + // fall back to position control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode("AUTO FOLLOW"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2962,10 +2992,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) { - - /* fall back to position control */ + // fall back to position control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_POSCTL; - print_reject_mode("AUTO HOLD"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2974,10 +3003,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_POSCTL) { - - /* fall back to altitude control */ + // fall back to altitude control + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_ALTCTL; - print_reject_mode("POSITION CONTROL"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2986,10 +3014,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) { - - /* fall back to stabilized */ + // fall back to stabilized + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_STAB; - print_reject_mode("ALTITUDE CONTROL"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -2998,10 +3025,9 @@ Commander::set_main_state_rc() } if (new_mode == commander_state_s::MAIN_STATE_STAB) { - - /* fall back to manual */ + // fall back to manual + print_reject_mode(new_mode); new_mode = commander_state_s::MAIN_STATE_MANUAL; - print_reject_mode("STABILIZED"); res = main_state_transition(_status, new_mode, _status_flags, &_internal_state); if (res != TRANSITION_DENIED) { @@ -3080,7 +3106,7 @@ Commander::set_main_state_rc() break; // changed successfully or already in this state } - print_reject_mode("POSITION CONTROL"); + print_reject_mode(commander_state_s::MAIN_STATE_POSCTL); } // fallback to ALTCTL @@ -3091,7 +3117,7 @@ Commander::set_main_state_rc() } if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) { - print_reject_mode("ALTITUDE CONTROL"); + print_reject_mode(commander_state_s::MAIN_STATE_ALTCTL); } // fallback to MANUAL @@ -3106,7 +3132,7 @@ Commander::set_main_state_rc() break; // changed successfully or already in this state } - print_reject_mode("AUTO MISSION"); + print_reject_mode(commander_state_s::MAIN_STATE_AUTO_MISSION); // fallback to LOITER if home position not set res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); @@ -3362,17 +3388,19 @@ 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(); + const hrt_abstime time_now_us = hrt_absolute_time(); - if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { - _last_print_mode_reject_time = t; - mavlink_log_critical(&_mavlink_log_pub, "REJECT %s", msg); + if (time_now_us > _last_print_mode_reject_time + PRINT_MODE_REJECT_INTERVAL) { + + mavlink_log_critical(&_mavlink_log_pub, "Rejecting %s mode", 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 = time_now_us; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ee5dfb3d14..77755a3b03 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -154,7 +154,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();