diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index b4b9c7814c..1c1d2e7a18 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 uint8 main_state # main state machine -uint8 main_state_prev # previous main state uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 05e8b1e590..9593ffb313 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -227,6 +227,8 @@ static bool cb_usb; static bool calibration_enabled = false; +static uint8_t main_state_prev = 0; + /** * The daemon app only briefly exists to start * 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 */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_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) { /* 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) { /* 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) { /* AUTO */ if (custom_sub_mode > 0) { switch(custom_sub_mode) { 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; 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; 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; 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; 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; case PX4_CUSTOM_SUB_MODE_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 { - 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) { /* 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) { /* 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) { /* 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) { /* 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 { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* 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) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* 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) { /* 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 { /* 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) { - 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) { print_reject_mode(status_local, "OFFBOARD"); @@ -1006,7 +1008,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { /* If the mavlink command is used to enable or disable offboard control: * 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; } } @@ -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: { /* 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!"); } else { 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: { /* 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!"); } else { warnx("landing denied"); @@ -1234,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; 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.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -2176,13 +2178,13 @@ int commander_thread_main(int argc, char *argv[]) break; } 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; } break; } 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; } 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.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 */ /* only switch back to at least altitude controlled modes */ - if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || - status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { + if (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || + main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF && mission_result.finished) || (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND && 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 /* if offboard is set already by a mavlink command, abort */ 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 */ @@ -2942,7 +2944,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switch overrides main switch */ 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) { 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 */ if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_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) { print_reject_mode(status_local, "AUTO RTL"); /* 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) { @@ -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 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) { - 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 { - 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 * rattitude mode.*/ 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 { - 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 { - 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 @@ -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 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) { 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 - 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) { 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 - 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 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,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) { 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"); } 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) { 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"); // 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) { 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 - 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) { break; // changed successfully or already in this state } // 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) { break; // changed successfully or already in this state } // 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 break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 67b57d8139..4304ef18d4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -339,7 +339,7 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } 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; @@ -403,7 +403,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } if (ret == TRANSITION_CHANGED) { 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; } else { ret = TRANSITION_NOT_CHANGED; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 805154f537..ca5a826e19 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,7 +67,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta bool circuit_breaker_engaged_power_check, 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);