zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Checkpoint: navigation state machine as discussed with Lorenz

sbg
Julian Oes 12 years ago
parent
0e29f2505a
commit
ebe0285ce7
7 changed files with 373 additions and 152 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. BIN
      Documentation/flight_mode_state_machine.odg
  2. 16
      apps/commander/commander.c
  3. 357
      apps/commander/state_machine_helper.c
  4. 3
      apps/commander/state_machine_helper.h
  5. 8
      apps/controllib/fixedwing.cpp
  6. 55
      apps/mavlink/mavlink.c
  7. 86
      apps/uORB/topics/vehicle_status.h

BIN
Documentation/flight_mode_state_machine.odg
View File

Binary file not shown.

16
apps/commander/commander.c
Unescape View File

@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */ /* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status)); 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.arming_state = ARMING_STATE_INIT;
current_status.hil_state = HIL_STATE_OFF; current_status.hil_state = HIL_STATE_OFF;
current_status.flag_system_armed = false; 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 */ /* Now it's time to handle the stick inputs */
navigation_state_update(stat_pub, &current_status, mavlink_fd);
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 );
}
}
}
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
if (!current_status.rc_signal_found_once) { if (!current_status.rc_signal_found_once) {

357
apps/commander/state_machine_helper.c
Unescape View File

@ -56,60 +56,195 @@
/** /**
* Transition from one sytem state to another * 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; 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) {
/* /* evaluate the navigation state when disarmed */
* Firstly evaluate mode switch position case ARMING_STATE_STANDBY:
* */
/* Always accept manual mode */ /* Always accept manual mode */
if (current_status->mode_switch == MODE_SWITCH_MANUAL) { if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
new_system_state = SYSTEM_STATE_MANUAL; new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
/* Accept SEATBELT if there is a local position estimate */ /* Accept SEATBELT if there is a local position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
if (current_status->flag_local_position_valid) { if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT; new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
} else { } else {
/* or just fall back to manual */ /* or just fall back to manual */
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL); new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
} }
/* Accept AUTO if there is a global position estimate */ /* Accept AUTO if there is a global position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_AUTO) { } else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
if (current_status->flag_local_position_valid) { if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT); new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
} else { } else {
mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position");
/* otherwise fallback to seatbelt or even manual */ /* otherwise fallback to seatbelt or even manual */
if (current_status->flag_local_position_valid) { if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT; new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY;
} else { } else {
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL; new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY;
}
}
} }
}
}
/* break;
* Next up, check for
*/ /* 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 */ /* 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 */ /* Check if the transition is valid */
if (system_state_update(current_status->system_state, new_system_state) == OK) { if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) {
current_status->system_state = new_system_state; current_status->navigation_state = new_navigation_state;
state_machine_publish(status_pub, current_status, mavlink_fd); state_machine_publish(status_pub, current_status, mavlink_fd);
} else { } else {
mavlink_log_critical(mavlink_fd, "System state transition not valid"); 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 * This functions does not evaluate any input flags but only checks if the transitions
* are valid. * 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; int ret = ERROR;
/* only check transition if the new state is actually different from the current one */ /* 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) { switch (new_navigation_state) {
case SYSTEM_STATE_INIT: case NAVIGATION_STATE_INIT:
/* transitions back to INIT are possible for calibration */ /* transitions back to INIT are possible for calibration */
if (current_system_state == SYSTEM_STATE_MANUAL if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_system_state == SYSTEM_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
|| current_system_state == SYSTEM_STATE_AUTO) { || 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; ret = OK;
} }
break;
case NAVIGATION_STATE_MANUAL:
/* all transitions possible */
ret = OK;
break; break;
case SYSTEM_STATE_MANUAL: case NAVIGATION_STATE_SEATBELT_STANDBY:
/* transitions from INIT or from other modes */ /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
if (current_system_state == SYSTEM_STATE_INIT if (current_navigation_state == NAVIGATION_STATE_INIT
|| current_system_state == SYSTEM_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_system_state == SYSTEM_STATE_AUTO) { || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_navigation_state == NAVIGATION_STATE_SEATBELT
|| current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) {
ret = OK; 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; break;
case SYSTEM_STATE_SEATBELT: case NAVIGATION_STATE_SEATBELT_DESCENT:
/* transitions from INIT or from other modes */ /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
if (current_system_state == SYSTEM_STATE_INIT if (current_navigation_state == NAVIGATION_STATE_SEATBELT
|| current_system_state == SYSTEM_STATE_MANUAL || current_navigation_state == NAVIGATION_STATE_MANUAL
|| current_system_state == SYSTEM_STATE_AUTO) { || 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; 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; break;
case SYSTEM_STATE_AUTO: case NAVIGATION_STATE_AUTO_READY:
/* transitions from INIT or from other modes */ /* transitions from AUTO_STANDBY or AUTO_LAND */
if (current_system_state == SYSTEM_STATE_INIT if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_system_state == SYSTEM_STATE_MANUAL || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) {
|| current_system_state == SYSTEM_STATE_SEATBELT) {
ret = OK; ret = OK;
} }
break; break;
case SYSTEM_STATE_REBOOT: case NAVIGATION_STATE_AUTO_TAKEOFF:
/* from INIT you can go straight to REBOOT */ /* only transitions from AUTO_READY */
if (current_system_state == SYSTEM_STATE_INIT) { if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = OK; ret = OK;
} }
break; 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 */ case NAVIGATION_STATE_AUTO_LAND:
if (current_system_state == SYSTEM_STATE_INIT) { /* 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; ret = OK;
} }

3
apps/commander/state_machine_helper.h
Unescape View File

@ -47,10 +47,11 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.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); //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); 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_ */ #endif /* STATE_MACHINE_HELPER_H_ */

8
apps/controllib/fixedwing.cpp
Unescape View File

@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update()
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f; _actuators.control[i] = 0.0f;
#warning this if is incomplete, should be based on flags
// only update guidance in auto mode // only update guidance in auto mode
if (_status.navigation_state == NAVIGATION_STATE_MISSION) { if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
// update guidance // update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd); _guide.update(_pos, _att, _posCmd, _lastPosCmd);
} }
@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update()
// once the system switches from manual or auto to stabilized // once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position // the setpoint should update to loitering around this position
#warning should be base on flags
// handle autopilot modes // handle autopilot modes
if (_status.navigation_state == NAVIGATION_STATE_MISSION || if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
_status.navigation_state == NAVIGATION_STATE_MANUAL) { _status.navigation_state == NAVIGATION_STATE_MANUAL) {
// update guidance // update guidance
@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_THR] : _manual.throttle; _actuators.control[CH_THR] : _manual.throttle;
} }
#warning should be base on flags
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {

55
apps/mavlink/mavlink.c
Unescape View File

@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
} }
/* autonomous mode */ /* autonomous mode */
if (v_status.navigation_state == NAVIGATION_STATE_MISSION if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
|| v_status.navigation_state == NAVIGATION_STATE_TAKEOFF || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_LAND || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
|| 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; *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
} }
/* set arming state */ /* 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; *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else { } else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *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; *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
} }
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { // if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
//
*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; // *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
} // }
//
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { // if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
//
*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; // *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; *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; *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; *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; *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; *mavlink_state = MAV_STATE_UNINIT;
} else { } else {

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

@ -60,13 +60,20 @@
/* State Machine */ /* State Machine */
typedef enum { typedef enum {
SYSTEM_STATE_INIT=0, NAVIGATION_STATE_INIT=0,
SYSTEM_STATE_MANUAL, NAVIGATION_STATE_MANUAL_STANDBY,
SYSTEM_STATE_SEATBELT, NAVIGATION_STATE_MANUAL,
SYSTEM_STATE_AUTO, NAVIGATION_STATE_SEATBELT_STANDBY,
SYSTEM_STATE_REBOOT, NAVIGATION_STATE_SEATBELT,
SYSTEM_STATE_IN_AIR_RESTORE NAVIGATION_STATE_SEATBELT_DESCENT,
} system_state_t; 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 { typedef enum {
MANUAL_STANDBY = 0, MANUAL_STANDBY = 0,
@ -75,50 +82,20 @@ typedef enum {
} manual_state_t; } manual_state_t;
typedef enum { typedef enum {
SEATBELT_STANDBY, ARMING_STATE_INIT = 0,
SEATBELT_READY, ARMING_STATE_STANDBY,
SEATBELT, ARMING_STATE_ARMED,
SEATBELT_ASCENT, ARMING_STATE_ARMED_ERROR,
SEATBELT_DESCENT, ARMING_STATE_STANDBY_ERROR,
} seatbelt_state_t; ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
typedef enum { } arming_state_t;
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;
typedef enum { typedef enum {
HIL_STATE_OFF = 0, HIL_STATE_OFF = 0,
HIL_STATE_ON HIL_STATE_ON
} hil_state_t; } 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 { typedef enum {
MODE_SWITCH_MANUAL = 0, MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_SEATBELT, MODE_SWITCH_SEATBELT,
@ -135,6 +112,17 @@ typedef enum {
MISSION_SWITCH_MISSION MISSION_SWITCH_MISSION
} mission_switch_pos_t; } 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 * 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_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED // uint64_t failsave_highlevel_begin; TO BE COMPLETED
system_state_t system_state; /**< current system state */ navigation_state_t navigation_state; /**< current system state */
// arming_state_t arming_state; /**< current arming state */ arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */ hil_state_t hil_state; /**< current hil state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ 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_arming_requested;
bool flag_system_disarming_requested; bool flag_system_disarming_requested;
bool flag_system_reboot_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_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */

Write Preview
Loading…
Cancel
Save