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

Checkpoint: navigation state machine as discussed with Lorenz

This commit is contained in:
Julian Oes 2013-02-21 13:06:56 -08:00
parent 0e29f2505a
commit ebe0285ce7
7 changed files with 393 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/flight_mode_state_machine.odg
View File

Binary file not shown.

16
apps/commander/commander.c
View File

@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_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, &current_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
do_navigation_state_update(stat_pub, &current_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, &current_status, mavlink_fd, NAVIGATION_STATE_MISSION );
}
}
}
navigation_state_update(stat_pub, &current_status, mavlink_fd);
/* handle the case where RC signal was regained */
if (!current_status.rc_signal_found_once) {

397
apps/commander/state_machine_helper.c
View File

@ -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");
/* 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_system_state = SYSTEM_STATE_SEATBELT;
} else {
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL;
/* 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) {
ret = OK;
}
break;
case SYSTEM_STATE_MANUAL:
/* 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) {
ret = OK;
}
break;
case SYSTEM_STATE_SEATBELT:
/* 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) {
ret = OK;
}
break;
case SYSTEM_STATE_AUTO:
/* 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) {
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 SYSTEM_STATE_REBOOT:
case NAVIGATION_STATE_MANUAL_STANDBY:
/* from INIT you can go straight to REBOOT */
if (current_system_state == SYSTEM_STATE_INIT) {
/* 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 SYSTEM_STATE_IN_AIR_RESTORE:
case NAVIGATION_STATE_MANUAL:
/* from INIT you can go straight to an IN AIR RESTORE */
if (current_system_state == SYSTEM_STATE_INIT) {
/* all transitions possible */
ret = OK;
break;
case NAVIGATION_STATE_SEATBELT_STANDBY:
/* 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 NAVIGATION_STATE_SEATBELT_DESCENT:
/* 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 NAVIGATION_STATE_AUTO_READY:
/* 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 NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = OK;
}
break;
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;
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;
}

3
apps/commander/state_machine_helper.h
View File

@ -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_ */

8
apps/controllib/fixedwing.cpp
View File

@ -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) {

55
apps/mavlink/mavlink.c
View File

@ -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 {

86
apps/uORB/topics/vehicle_status.h
View File

@ -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 */