dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: centralize main_state strings and use for rejection message

This commit is contained in:
Daniel Agar 2021-03-29 11:39:00 -04:00
parent 0f29b87101
commit e635fccd25
2 changed files with 67 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
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 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;
}
}

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

@ -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();