Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

commander: take previous main state out of status

Browse Source
This commit is contained in:
Julian Oes
2016-02-25 08:11:57 +00:00
parent 181eb49da8
commit 141b984d5b
4 changed files with 51 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_status.msg
-1
View File
@@ -60,7 +60,6 @@ uint8 RC_IN_MODE_GENERATED = 2
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine uint8 main_state # main state machine
uint8 main_state_prev # previous main state
uint8 nav_state # set navigation state machine to specified value uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state uint8 arming_state # current arming state
uint8 hil_state # current hil state uint8 hil_state # current hil state
src/modules/commander/commander.cpp
+48 -46
View File
@@ -227,6 +227,8 @@ static bool cb_usb;
static bool calibration_enabled = false; static bool calibration_enabled = false;
static uint8_t main_state_prev = 0;
/** /**
* The daemon app only briefly exists to start * The daemon app only briefly exists to start
* the background job. The stack size assigned in the * the background job. The stack size assigned in the
@@ -704,34 +706,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */ /* ALTCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */ /* AUTO */
if (custom_sub_mode > 0) { if (custom_sub_mode > 0) {
switch(custom_sub_mode) { switch(custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL: case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND: case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev);
break; break;
case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET);
@@ -744,12 +746,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} }
} else { } else {
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
} }
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */ /* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
/* RATTITUDE */ /* RATTITUDE */
@@ -757,30 +759,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */ /* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */ /* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
} }
} else { } else {
/* use base mode */ /* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */ /* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else { } else {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
} }
} }
} }
@@ -992,7 +994,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} }
if (cmd->param1 > 0.5f) { if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode(status_local, "OFFBOARD");
@@ -1006,7 +1008,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else { } else {
/* If the mavlink command is used to enable or disable offboard control: /* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */ * switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard); res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev);
status_local->offboard_control_set_by_command = false; status_local->offboard_control_set_by_command = false;
} }
} }
@@ -1014,7 +1016,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */ /* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF)) { if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev)) {
warnx("taking off!"); warnx("taking off!");
} else { } else {
warnx("takeoff denied"); warnx("takeoff denied");
@@ -1025,7 +1027,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
/* ok, home set, use it to take off */ /* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND)) { if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev)) {
warnx("landing!"); warnx("landing!");
} else { } else {
warnx("landing denied"); warnx("landing denied");
@@ -1234,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[])
status.rc_input_blocked = false; status.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX; main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT; status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
@@ -2176,13 +2178,13 @@ int commander_thread_main(int argc, char *argv[])
break; break;
} }
case (geofence_result_s::GF_ACTION_LOITER) : { case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) { if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev)) {
geofence_loiter_on = true; geofence_loiter_on = true;
} }
break; break;
} }
case (geofence_result_s::GF_ACTION_RTL) : { case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) { if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev)) {
geofence_rtl_on = true; geofence_rtl_on = true;
} }
break; break;
@@ -2229,7 +2231,7 @@ int commander_thread_main(int argc, char *argv[])
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation); main_state_transition(&status, geofence_main_state_before_violation, main_state_prev);
} }
} }
} }
@@ -2541,15 +2543,15 @@ int commander_thread_main(int argc, char *argv[])
/* reset main state after takeoff or land has been completed */ /* reset main state after takeoff or land has been completed */
/* only switch back to at least altitude controlled modes */ /* only switch back to at least altitude controlled modes */
if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || if (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL ||
status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) {
if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) || && mission_result.finished) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.condition_landed)) { && status.condition_landed)) {
main_state_transition(&status, status.main_state_prev); main_state_transition(&status, main_state_prev, main_state_prev);
} }
} }
@@ -2913,7 +2915,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
// just delete this and respond to mode switches // just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */ /* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) { if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
} }
/* manual setpoint has not updated, do not re-evaluate it */ /* manual setpoint has not updated, do not re-evaluate it */
@@ -2942,7 +2944,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */ /* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode(status_local, "OFFBOARD");
@@ -2957,13 +2959,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* RTL switch overrides main switch */ /* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
warnx("RTL switch changed and ON!"); warnx("RTL switch changed and ON!");
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
if (res == TRANSITION_DENIED) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO RTL"); print_reject_mode(status_local, "AUTO RTL");
/* fallback to LOITER if home position not set */ /* fallback to LOITER if home position not set */
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
} }
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
@@ -3076,11 +3078,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
*/ */
// XXX: put ACRO and STAB on separate switches // XXX: put ACRO and STAB on separate switches
if (status.is_rotary_wing && !status.is_vtol) { if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
} else if (!status.is_rotary_wing) { } else if (!status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
} }
} }
@@ -3088,12 +3090,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* Similar to acro transitions for multirotors. FW aircraft don't need a /* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/ * rattitude mode.*/
if (status.is_rotary_wing) { if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE, main_state_prev);
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} }
}else { }else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
} }
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
@@ -3101,7 +3103,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3111,7 +3113,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode break; // changed successfully or already in this mode
@@ -3122,13 +3124,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3137,7 +3139,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO PAUSE"); print_reject_mode(status_local, "AUTO PAUSE");
} else { } else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3146,7 +3148,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO MISSION"); print_reject_mode(status_local, "AUTO MISSION");
// fallback to LOITER if home position not set // fallback to LOITER if home position not set
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
@@ -3154,21 +3156,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} }
// fallback to POSCTL // fallback to POSCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to ALTCTL // fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to MANUAL // fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
// TRANSITION_DENIED is not possible here // TRANSITION_DENIED is not possible here
break; break;
src/modules/commander/state_machine_helper.cpp
+2 -2
View File
@@ -339,7 +339,7 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
} }
transition_result_t transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev)
{ {
transition_result_t ret = TRANSITION_DENIED; transition_result_t ret = TRANSITION_DENIED;
@@ -403,7 +403,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
} }
if (ret == TRANSITION_CHANGED) { if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) { if (status->main_state != new_main_state) {
status->main_state_prev = status->main_state; main_state_prev = status->main_state;
status->main_state = new_main_state; status->main_state = new_main_state;
} else { } else {
ret = TRANSITION_NOT_CHANGED; ret = TRANSITION_NOT_CHANGED;
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -67,7 +67,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta
bool circuit_breaker_engaged_power_check, bool circuit_breaker_engaged_power_check,
bool cb_usb); bool cb_usb);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub); transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);