From 5ca5af5fcd2a2b352ad4a7707767778c6f3e295e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Feb 2016 15:41:03 +0000 Subject: [PATCH] commander: take main_state out of vehicle_status This state is only commander internal. Therefore it doesn't need to be in vehicle_status. Instead it is now in the commander_state message. --- msg/commander_state.msg | 31 +++ msg/vehicle_status.msg | 1 - src/drivers/blinkm/blinkm.cpp | 15 +- src/modules/commander/commander.cpp | 193 +++++++++--------- .../commander/state_machine_helper.cpp | 107 +++++----- src/modules/commander/state_machine_helper.h | 6 +- src/modules/sdlog2/sdlog2.c | 17 +- src/modules/sdlog2/sdlog2_messages.h | 5 +- src/modules/uORB/objects_common.cpp | 3 + 9 files changed, 214 insertions(+), 164 deletions(-) create mode 100644 msg/commander_state.msg diff --git a/msg/commander_state.msg b/msg/commander_state.msg new file mode 100644 index 0000000000..7a6f45cb38 --- /dev/null +++ b/msg/commander_state.msg @@ -0,0 +1,31 @@ +# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. +uint8 MAIN_STATE_MANUAL = 0 +uint8 MAIN_STATE_ALTCTL = 1 +uint8 MAIN_STATE_POSCTL = 2 +uint8 MAIN_STATE_AUTO_MISSION = 3 +uint8 MAIN_STATE_AUTO_LOITER = 4 +uint8 MAIN_STATE_AUTO_RTL = 5 +uint8 MAIN_STATE_ACRO = 6 +uint8 MAIN_STATE_OFFBOARD = 7 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_AUTO_TAKEOFF = 10 +uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_MAX = 12 + +# If you change the order, add or remove arming_state_t states make sure to update the arrays +# in state_machine_helper.cpp as well. +uint8 ARMING_STATE_INIT = 0 +uint8 ARMING_STATE_STANDBY = 1 +uint8 ARMING_STATE_ARMED = 2 +uint8 ARMING_STATE_ARMED_ERROR = 3 +uint8 ARMING_STATE_STANDBY_ERROR = 4 +uint8 ARMING_STATE_REBOOT = 5 +uint8 ARMING_STATE_IN_AIR_RESTORE = 6 +uint8 ARMING_STATE_MAX = 7 + +# state machine / state of vehicle. +# Encodes the complete system state and is set by the commander app. +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +uint8 main_state # main state machine diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 96aad6f90a..8f7983c17c 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -59,7 +59,6 @@ uint8 RC_IN_MODE_GENERATED = 2 # Encodes the complete system state and is set by the commander app. uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data -uint8 main_state # main state machine 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/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b2098f06c5..c69dae676a 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -701,19 +701,18 @@ BlinkM::led() led_blink = LED_BLINK; if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - /* indicate main control state */ - if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) { - led_color_4 = LED_GREEN; - } - /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { + /* indicate main control state */ + if (vehicle_control_mode.flag_control_auto_enabled) { led_color_4 = LED_BLUE; - } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) { + } else if (vehicle_control_mode.flag_control_position_enabled) { + led_color_4 = LED_GREEN; + + } else if (vehicle_control_mode.flag_control_altitude_enabled) { led_color_4 = LED_YELLOW; - } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) { + } else if (vehicle_control_mode.flag_control_manual_enabled) { led_color_4 = LED_WHITE; } else { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 26f39f64a3..096d095199 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -213,6 +213,7 @@ static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; +static struct commander_state_s internal_state; static unsigned _last_mission_instance = 0; static manual_control_setpoint_s _last_sp_man = {}; @@ -579,7 +580,7 @@ void print_status() const char *armed_str; - switch (state.arming_state) { + switch (status.arming_state) { case vehicle_status_s::ARMING_STATE_INIT: armed_str = "INIT"; break; @@ -698,34 +699,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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_LAND: - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET); @@ -738,12 +739,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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); } } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { /* RATTITUDE */ @@ -751,30 +752,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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); } } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); } 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_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* STABILIZED */ - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } else { /* MANUAL */ - main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); + main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } } } @@ -809,7 +810,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { - status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; + status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } else { @@ -979,14 +980,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = vehicle_status_s::MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard = commander_state_s::MAIN_STATE_MANUAL; - if (status_local->main_state != vehicle_status_s::MAIN_STATE_OFFBOARD) { - main_state_pre_offboard = status_local->main_state; + if (internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = internal_state.main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -1000,7 +1001,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, main_state_prev, &status_flags); + res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags, &internal_state); status_flags.offboard_control_set_by_command = false; } } @@ -1008,7 +1009,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, main_state_prev, &status_flags)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) { warnx("taking off!"); } else { warnx("takeoff denied"); @@ -1019,7 +1020,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, main_state_prev, &status_flags)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { warnx("landing!"); } else { warnx("landing denied"); @@ -1172,16 +1173,16 @@ int commander_thread_main(int argc, char *argv[]) // These are too verbose, but we will retain them a little longer // until we are sure we really don't need them. - // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; - // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; - // main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; - // main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; - // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - // main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; - // main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; - // main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; + // const char *main_states_str[commander_state_s::MAIN_STATE_MAX]; + // main_states_str[commander_state_s::MAIN_STATE_MANUAL] = "MANUAL"; + // main_states_str[commander_state_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + // main_states_str[commander_state_s::MAIN_STATE_POSCTL] = "POSCTL"; + // main_states_str[commander_state_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + // main_states_str[commander_state_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + // main_states_str[commander_state_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + // main_states_str[commander_state_s::MAIN_STATE_ACRO] = "ACRO"; + // main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB"; + // main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; @@ -1230,8 +1231,8 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status_flags.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; - status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; - main_state_prev = vehicle_status_s::MAIN_STATE_MAX; + internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL; + main_state_prev = commander_state_s::MAIN_STATE_MAX; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -2144,7 +2145,7 @@ int commander_thread_main(int argc, char *argv[]) static bool geofence_loiter_on = false; static bool geofence_rtl_on = false; - static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX; + static uint8_t geofence_main_state_before_violation = commander_state_s::MAIN_STATE_MAX; // check for geofence violation if (geofence_result.geofence_violated) { @@ -2164,13 +2165,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, main_state_prev, &status_flags)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state)) { 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, main_state_prev, &status_flags)) { + if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { geofence_rtl_on = true; } break; @@ -2188,26 +2189,26 @@ int commander_thread_main(int argc, char *argv[]) // reset if no longer in LOITER or if manually switched to LOITER geofence_loiter_on = geofence_loiter_on - && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER) + && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF); // reset if no longer in RTL or if manually switched to RTL geofence_rtl_on = geofence_rtl_on - && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL) + && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL) && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF); if (!geofence_loiter_on && !geofence_rtl_on) { // store the last good main_state when not in a geofence triggered action (LOITER or RTL) - geofence_main_state_before_violation = status.main_state; + geofence_main_state_before_violation = internal_state.main_state; } // revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST if ((geofence_loiter_on || geofence_rtl_on) && - (geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL || - geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL || - geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL || - geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO || - geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) { + (geofence_main_state_before_violation == commander_state_s::MAIN_STATE_MANUAL || + geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ALTCTL || + geofence_main_state_before_violation == commander_state_s::MAIN_STATE_POSCTL || + geofence_main_state_before_violation == commander_state_s::MAIN_STATE_ACRO || + geofence_main_state_before_violation == commander_state_s::MAIN_STATE_STAB)) { // transition to previous state if sticks are increased const float min_stick_change = 0.2; @@ -2217,7 +2218,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_prev, &status_flags); + main_state_transition(&status, geofence_main_state_before_violation, main_state_prev, &status_flags, &internal_state); } } } @@ -2286,10 +2287,10 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || - status.main_state == vehicle_status_s::MAIN_STATE_ACRO || - status.main_state == vehicle_status_s::MAIN_STATE_STAB || - status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || + (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || + internal_state.main_state == commander_state_s::MAIN_STATE_STAB || + internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || land_detector.landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { @@ -2328,10 +2329,10 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ - if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) - && (status.main_state != vehicle_status_s::MAIN_STATE_ACRO) - && (status.main_state != vehicle_status_s::MAIN_STATE_STAB) - && (status.main_state != vehicle_status_s::MAIN_STATE_ALTCTL)) { + if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) + && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) + && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB) + && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)) { print_reject_arm("NOT ARMING: Switch to a manual mode first."); } else if (!status_flags.condition_home_position_valid && @@ -2525,15 +2526,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 (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || - main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { + if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL || + main_state_prev == commander_state_s::MAIN_STATE_ALTCTL) { - if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + if ((internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && mission_result.finished) || - (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND && land_detector.landed)) { - main_state_transition(&status, main_state_prev, main_state_prev, &status_flags); + main_state_transition(&status, main_state_prev, main_state_prev, &status_flags, &internal_state); } } @@ -2556,12 +2557,12 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ - if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && - status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && - status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && - status.main_state !=vehicle_status_s::MAIN_STATE_STAB && - status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && - status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && + if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && + internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && + internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && + internal_state.main_state != commander_state_s::MAIN_STATE_STAB && + internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && + internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status_flags.gps_failure) || (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; @@ -2582,12 +2583,12 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the rc signal and the gps system have been checked * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ - if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || - status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || - status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || - status.main_state ==vehicle_status_s::MAIN_STATE_STAB || - status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || - status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && + if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || + internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || + internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + internal_state.main_state == commander_state_s::MAIN_STATE_STAB || + internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || + internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status_flags.gps_failure) || (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; @@ -2629,7 +2630,9 @@ int commander_thread_main(int argc, char *argv[]) } /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + bool nav_state_changed = set_nav_state(&status, + &internal_state, + (bool)datalink_loss_enabled, mission_result.finished, mission_result.stay_in_failsafe, &status_flags, @@ -2833,7 +2836,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->failsafe) { + if (status.failsafe) { rgbled_set_color(RGBLED_COLOR_PURPLE); } else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); @@ -2895,7 +2898,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_flags.offboard_control_set_by_command) { - return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags); + return main_state_transition(status_local,commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); } /* manual setpoint has not updated, do not re-evaluate it */ @@ -2924,7 +2927,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2939,13 +2942,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state); 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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); } if (res != TRANSITION_DENIED) { @@ -3058,11 +3061,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); } else if (!status.is_rotary_wing) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } else { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } } @@ -3070,12 +3073,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); } else { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } }else { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } // TRANSITION_DENIED is not possible here @@ -3083,7 +3086,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3093,7 +3096,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode @@ -3104,13 +3107,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); // 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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3119,7 +3122,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3128,7 +3131,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3136,21 +3139,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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); 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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); 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, main_state_prev, &status_flags); + res = main_state_transition(status_local,commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); // 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 a9e79432d4..a190243538 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,20 @@ 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, uint8_t &main_state_prev, - status_flags_s *status_flags) + status_flags_s *status_flags, struct commander_state_s *internal_state) { transition_result_t ret = TRANSITION_DENIED; /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ACRO: - case vehicle_status_s::MAIN_STATE_RATTITUDE: - case vehicle_status_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_MANUAL: + case commander_state_s::MAIN_STATE_ACRO: + case commander_state_s::MAIN_STATE_RATTITUDE: + case commander_state_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; - case vehicle_status_s::MAIN_STATE_ALTCTL: + case commander_state_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || @@ -371,7 +371,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case vehicle_status_s::MAIN_STATE_POSCTL: + case commander_state_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status_flags->condition_local_position_valid || status_flags->condition_global_position_valid) { @@ -379,25 +379,25 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + case commander_state_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status_flags->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; - case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: - case vehicle_status_s::MAIN_STATE_AUTO_LAND: + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + case commander_state_s::MAIN_STATE_AUTO_MISSION: + case commander_state_s::MAIN_STATE_AUTO_RTL: + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + case commander_state_s::MAIN_STATE_AUTO_LAND: /* need global position and home position */ if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { ret = TRANSITION_CHANGED; } break; - case vehicle_status_s::MAIN_STATE_OFFBOARD: + case commander_state_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ if (!status_flags->offboard_control_signal_lost) { @@ -406,14 +406,14 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case vehicle_status_s::MAIN_STATE_MAX: + case commander_state_s::MAIN_STATE_MAX: default: break; } if (ret == TRANSITION_CHANGED) { - if (status->main_state != new_main_state) { - main_state_prev = status->main_state; - status->main_state = new_main_state; + if (internal_state->main_state != new_main_state) { + main_state_prev = internal_state->main_state; + internal_state->main_state = new_main_state; } else { ret = TRANSITION_NOT_CHANGED; } @@ -597,25 +597,26 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, +bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, + const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed) { navigation_state_t nav_state_old = status->nav_state; bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); - status->failsafe = false; + //status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_RATTITUDE: - case vehicle_status_s::MAIN_STATE_STAB: - case vehicle_status_s::MAIN_STATE_ALTCTL: - case vehicle_status_s::MAIN_STATE_POSCTL: + switch (internal_state->main_state) { + case commander_state_s::MAIN_STATE_ACRO: + case commander_state_s::MAIN_STATE_MANUAL: + case commander_state_s::MAIN_STATE_RATTITUDE: + case commander_state_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_ALTCTL: + case commander_state_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; @@ -628,28 +629,28 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } } else { - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: + switch (internal_state->main_state) { + case commander_state_s::MAIN_STATE_ACRO: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; break; - case vehicle_status_s::MAIN_STATE_MANUAL: + case commander_state_s::MAIN_STATE_MANUAL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case vehicle_status_s::MAIN_STATE_RATTITUDE: + case commander_state_s::MAIN_STATE_RATTITUDE: status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; break; - case vehicle_status_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_STAB: status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; - case vehicle_status_s::MAIN_STATE_ALTCTL: + case commander_state_s::MAIN_STATE_ALTCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case vehicle_status_s::MAIN_STATE_POSCTL: + case commander_state_s::MAIN_STATE_POSCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; break; @@ -660,7 +661,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case commander_state_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe * - if commanded to do so @@ -693,7 +694,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ } else if (data_link_loss_enabled && status->data_link_lost) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; @@ -710,7 +711,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || (status->rc_signal_lost && mission_finished))) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; @@ -728,13 +729,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + case commander_state_s::MAIN_STATE_AUTO_LOITER: /* go into failsafe on a engine failure */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; @@ -748,7 +749,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe if RC is lost and datalink loss is not set up */ } else if (status->rc_signal_lost && !data_link_loss_enabled) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; @@ -771,14 +772,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_AUTO_RTL: + case commander_state_s::MAIN_STATE_AUTO_RTL: /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -792,17 +793,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: /* require global position and home */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (!status->condition_global_position_valid) { + } else if (!status_flags->condition_global_position_valid) { status->failsafe = true; - if (status->condition_local_position_valid) { + if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } else if (status->condition_local_altitude_valid) { + } else if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; @@ -819,7 +820,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -833,14 +834,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_AUTO_LAND: + case commander_state_s::MAIN_STATE_AUTO_LAND: /* require global position and home */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status_flags->condition_global_position_valid || !status_flags->condition_home_position_valid)) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; @@ -852,14 +853,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; - case vehicle_status_s::MAIN_STATE_OFFBOARD: + case commander_state_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) { - status->failsafe = true; + //status->failsafe = true; status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) { - status->failsafe = true; + //status->failsafe = true; if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b8805f17d4..72cc961487 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,6 +46,7 @@ #include #include #include +#include typedef enum { TRANSITION_DENIED = -1, @@ -103,11 +104,12 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, - status_flags_s *status_flags); + status_flags_s *status_flags, struct commander_state_s *internal_state); 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); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, +bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state, + const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed); int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c971fdf505..c9fb93c6b3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -114,6 +114,7 @@ #include #include #include +#include #include #include @@ -1159,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct camera_trigger_s camera_trigger; struct ekf2_replay_s replay; struct vehicle_land_detected_s land_detected; + struct commander_state_s commander_state; } buf; memset(&buf, 0, sizeof(buf)); @@ -1265,6 +1267,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int cam_trig_sub; int replay_sub; int land_detected_sub; + int commander_state_sub; } subs; subs.cmd_sub = -1; @@ -1305,6 +1308,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cam_trig_sub = -1; subs.replay_sub = -1; subs.land_detected_sub = -1; + subs.commander_state_sub = -1; /* add new topics HERE */ @@ -1424,6 +1428,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status); + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + bool commander_state_updated = copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub, + &buf.commander_state); + if (status_updated) { if (log_when_armed) { handle_status(&buf_status); @@ -1447,10 +1455,13 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TIME.t = hrt_absolute_time(); LOGBUFFER_WRITE_AND_COUNT(TIME); - /* --- VEHICLE STATUS --- */ - if (status_updated) { + /* --- VEHICLE STATUS / COMMANDER DEBUGGING --- */ + if (status_updated || commander_state_updated) { log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = buf_status.main_state; + // TODO: This field should get DEPRECATED in favor of nav_state. main_state is only for + // commander debugging. + log_msg.body.log_STAT.main_state = buf.commander_state.main_state; + log_msg.body.log_STAT.nav_state = buf_status.nav_state; log_msg.body.log_STAT.arming_state = buf_status.arming_state; log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.load = buf_status.load; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 95dfa2d7e2..7b0954648c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -178,6 +178,7 @@ struct log_ATTC_s { #define LOG_STAT_MSG 10 struct log_STAT_s { uint8_t main_state; + uint8_t nav_state; uint8_t arming_state; uint8_t failsafe; float load; @@ -627,7 +628,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBf", "MainState,ArmS,Failsafe,Load"), + LOG_FORMAT(STAT, "BBBBf", "MainState,NavState,ArmS,Failsafe,Load"), LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 2ee138ea7c..1f0f08a931 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -288,3 +288,6 @@ ORB_DEFINE(mavlink_log, struct mavlink_log_s); #include "topics/follow_target.h" ORB_DEFINE(follow_target, struct follow_target_s); + +#include "topics/commander_state.h" +ORB_DEFINE(commander_state, struct commander_state_s);