diff options
| -rw-r--r-- | Documentation/flight_mode_state_machine.odg | bin | 24059 -> 23463 bytes | |||
| -rw-r--r-- | apps/commander/commander.c | 16 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 357 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.h | 3 | ||||
| -rw-r--r-- | apps/controllib/fixedwing.cpp | 8 | ||||
| -rw-r--r-- | apps/mavlink/mavlink.c | 55 | ||||
| -rw-r--r-- | apps/uORB/topics/vehicle_status.h | 86 |
7 files changed, 373 insertions, 152 deletions
diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg Binary files differindex a50366bcb..cbdad91c9 100644 --- a/Documentation/flight_mode_state_machine.odg +++ b/Documentation/flight_mode_state_machine.odg diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8b9e7c49c..4e2b4907b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_STANDBY; + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; @@ -1857,19 +1857,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - - if (current_status.arming_state == ARMING_STATE_ARMED) { - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); - } - } - } + navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2e4935471..e6344f1a8 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,60 +56,195 @@ /** * Transition from one sytem state to another */ -void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { int ret = ERROR; - system_state_t new_system_state; + navigation_state_t new_navigation_state; + /* do the navigation state update depending on the arming state */ + switch (current_status->arming_state) { - /* - * Firstly evaluate mode switch position - * */ + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_system_state = SYSTEM_STATE_MANUAL; + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL); - } + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT); - } else { - mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL; + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } + } } - } - } - /* - * Next up, check for - */ + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_NONE) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_RETURN) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + /* descent not possible without baro information, fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept LOITER if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_NONE) { + + if (current_status->flag_global_position_valid) { + new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Accept MISSION if there is a global position estimate and home position */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_MISSION) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Go to RTL or land if global position estimate and home position is available */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_RETURN + && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + + /* after RTL go to LAND */ + if (current_status->flag_system_returned_to_home) { + new_navigation_state = NAVIGATION_STATE_AUTO_LAND; + } else { + new_navigation_state = NAVIGATION_STATE_AUTO_RTL; + } + + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + } + break; + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } /* Update the system state in case it has changed */ - if (current_status->system_state != new_system_state) { + if (current_status->navigation_state != new_navigation_state) { /* Check if the transition is valid */ - if (system_state_update(current_status->system_state, new_system_state) == OK) { - current_status->system_state = new_system_state; + if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { + current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); @@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_system_state != current_system_state) { + if (new_navigation_state != current_navigation_state) { - switch (new_system_state) { - case SYSTEM_STATE_INIT: + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { + if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } + break; + case NAVIGATION_STATE_MANUAL: + + /* all transitions possible */ + ret = OK; break; - case SYSTEM_STATE_MANUAL: + case NAVIGATION_STATE_SEATBELT_STANDBY: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { ret = OK; } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + ret = OK; + } break; - case SYSTEM_STATE_SEATBELT: + case NAVIGATION_STATE_SEATBELT_DESCENT: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_AUTO) { + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { ret = OK; } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } break; - case SYSTEM_STATE_AUTO: + case NAVIGATION_STATE_AUTO_READY: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT) { + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { ret = OK; } break; - case SYSTEM_STATE_REBOOT: + case NAVIGATION_STATE_AUTO_TAKEOFF: - /* from INIT you can go straight to REBOOT */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* only transitions from AUTO_READY */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } break; - case SYSTEM_STATE_IN_AIR_RESTORE: + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; - /* from INIT you can go straight to an IN AIR RESTORE */ - if (current_system_state == SYSTEM_STATE_INIT) { + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 57b3db8f1..1c0564d07 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,10 +47,11 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9a6919535..584ca2306 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_MISSION || + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance @@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - +#warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e25f1be27..34b267d56 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (v_status.flag_system_armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } - - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } +// +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } /** @@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { + } else if (v_status.flag_system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_INIT) { + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 20cb25cc0..29baff34b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,13 +60,20 @@ /* State Machine */ typedef enum { - SYSTEM_STATE_INIT=0, - SYSTEM_STATE_MANUAL, - SYSTEM_STATE_SEATBELT, - SYSTEM_STATE_AUTO, - SYSTEM_STATE_REBOOT, - SYSTEM_STATE_IN_AIR_RESTORE -} system_state_t; + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; typedef enum { MANUAL_STANDBY = 0, @@ -75,50 +82,20 @@ typedef enum { } manual_state_t; typedef enum { - SEATBELT_STANDBY, - SEATBELT_READY, - SEATBELT, - SEATBELT_ASCENT, - SEATBELT_DESCENT, -} seatbelt_state_t; - -typedef enum { - AUTO_STANDBY, - AUTO_READY, - AUTO_LOITER, - AUTO_MISSION, - AUTO_RTL, - AUTO_TAKEOFF, - AUTO_LAND, -} auto_state_t; - -//typedef enum { -// ARMING_STATE_INIT = 0, -// ARMING_STATE_STANDBY, -// ARMING_STATE_ARMED_GROUND, -// ARMING_STATE_ARMED_AIRBORNE, -// ARMING_STATE_ERROR_GROUND, -// ARMING_STATE_ERROR_AIRBORNE, -// ARMING_STATE_REBOOT, -// ARMING_STATE_IN_AIR_RESTORE -//} arming_state_t; + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; typedef enum { HIL_STATE_OFF = 0, HIL_STATE_ON } hil_state_t; -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_SEATBELT, @@ -135,6 +112,17 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -181,8 +169,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - system_state_t system_state; /**< current system state */ -// arming_state_t arming_state; /**< current arming state */ + navigation_state_t navigation_state; /**< current system state */ + arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -203,7 +191,9 @@ struct vehicle_status_s bool flag_system_arming_requested; bool flag_system_disarming_requested; bool flag_system_reboot_requested; - bool flag_system_on_ground; + bool flag_system_returned_to_home; + + bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ |