fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 21:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP)

Browse Source
This commit is contained in:
Julian Oes
2014-05-27 21:56:32 +02:00
parent 063caba36b
commit ed6c2a5168
11 changed files with 423 additions and 917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+222 -266
View File
@@ -1,11 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -38,8 +33,13 @@
/**
* @file commander.cpp
* Main system state machine implementation.
* Main fail-safe handling.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -76,6 +76,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/mission_result.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -87,6 +88,7 @@
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{
/* result of the command */
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
return false;
}
/* only handle high-priority commands here */
/* result of the command */
enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
uint8_t base_mode = (uint8_t)cmd->param1;
uint8_t custom_main_mode = (uint8_t)cmd->param2;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
}
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
// Transition the arming state
transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
// Transition the arming state
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (hil_ret == OK) {
ret = true;
}
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_res = main_state_transition(status, MAIN_STATE_ACRO);
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
}
} else {
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
}
}
}
if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
@@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
@@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAVIGATION_STATE_LOITER;
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->set_nav_state = NAVIGATION_STATE_MISSION;
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
@@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
/* reject parachute depoyment not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
home->timestamp = hrt_absolute_time();
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
@@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
home->timestamp = hrt_absolute_time();
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
@@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
answer_command(*cmd, cmd_result);
}
/* send any requested ACKs */
if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* send acknowledge command */
// XXX TODO
}
@@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[])
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = NAVIGATION_STATE_NONE;
status.set_nav_state = NAVIGATION_STATE_MANUAL;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[])
safety.safety_switch_available = false;
safety.safety_off = false;
/* Subscribe to mission result topic */
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[])
start_time = hrt_absolute_time();
transition_result_t arming_ret;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_state_changed = false;
while (!thread_should_exit) {
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */
orb_check(param_changed_sub, &updated);
@@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[])
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
arming_state_changed = true;
}
}
}
@@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 1");
arming_state_changed = true;
}
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 2");
arming_state_changed = true;
}
}
status_changed = true;
}
@@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
/* TODO: check for sensors */
warnx("arming status1: %d", status.arming_state);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
warnx("arming status2: %d", status.arming_state);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed");
arming_state_changed = true;
}
} else {
// XXX: Add emergency stuff if sensors are lost
/* TODO: Add emergency stuff if sensors are lost */
}
@@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
}
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
}
/* start RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
@@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
transition_result_t arming_res; // store all transitions results here
/* arm/disarm by RC */
arming_res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
@@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("stick 1");
arming_state_changed = true;
}
stick_off_counter = 0;
} else {
@@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("stick 2");
arming_state_changed = true;
}
}
stick_on_counter = 0;
@@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter = 0;
}
if (arming_res == TRANSITION_CHANGED) {
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
arming_state_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
// /* recover from failsafe */
// (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
// }
/* evaluate the main state machine according to mode switches */
transition_result_t main_res = set_main_state_rc(&status, &sp_man);
@@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[])
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED) {
tune_positive(armed.armed);
main_state_changed = true;
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
/* set navigation state */
/* RETURN switch, overrides MISSION switch */
if (sp_man.return_switch == SWITCH_POS_ON) {
/* switch to RTL if not already landed after RTL and home position set */
status.set_nav_state = NAVIGATION_STATE_RTL;
} else {
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAVIGATION_STATE_LOITER;
} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
/* stick is in MISSION position */
status.set_nav_state = NAVIGATION_STATE_MISSION;
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAVIGATION_STATE_MISSION;
}
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
if (armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
if (auto_res == TRANSITION_NOT_CHANGED) {
last_auto_state_valid = hrt_absolute_time();
}
/* still invalid state after the timeout interval, execute failsafe */
if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (auto_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
} else {
status.set_nav_state = NAVIGATION_STATE_MISSION;
}
} else {
/* failsafe for manual modes */
transition_result_t manual_res = TRANSITION_DENIED;
if (!status.condition_landed) {
/* vehicle is not landed, try to perform RTL */
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
}
if (manual_res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
} else {
status.set_nav_state = NAVIGATION_STATE_RTL;
}
}
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
// TODO remove this hack
/* flight termination in manual mode if assist switch is on POSCTL position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
if (armed.armed) {
/* if RC is lost, switch to RTL_RC or Termination unless a mission is running
* and not yet finished) */
if (status.rc_signal_lost
&& !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
/* if we have a global position, we can switch to RTL, if not, we can try to land */
if (status.condition_global_position_valid) {
status.failsafe_state = FAILSAFE_STATE_RTL_RC;
} else {
status.failsafe_state = FAILSAFE_STATE_LAND;
}
}
/* if the data link is lost, switch to RTL_DL or Termination */
/* TODO: add this */
} else {
/* reset failsafe when disarmed */
status.failsafe_state = FAILSAFE_STATE_NORMAL;
}
/* handle commands last, as the system needs to be updated to handle them */
@@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool failsafe_state_changed = check_failsafe_state_changed();
hrt_abstime t1 = hrt_absolute_time();
/* print new state */
@@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
}
arming_state_changed = false;
}
was_armed = armed.armed;
/* now set nav state according to failsafe and main state */
set_nav_state(&status);
if (main_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
main_state_changed = false;
}
if (failsafe_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
failsafe_state_changed = false;
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
warnx("NONE");
break;
case SWITCH_POS_OFF: // MANUAL
@@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break;
case SWITCH_POS_ON: // AUTO
res = main_state_transition(status, MAIN_STATE_AUTO);
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to ALTCTL (POSCTL likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
}
void
set_control_mode()
{
/* set vehicle_control_mode according to main state and failsafe state */
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
/* TODO: check this */
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator should act */
bool navigator_enabled = false;
switch (status.failsafe_state) {
case FAILSAFE_STATE_NORMAL:
switch (status.main_state) {
case MAIN_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
navigator_enabled = true;
break;
case MAIN_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
default:
break;
}
switch (status.set_nav_state) {
case NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_RTL:
navigator_enabled = true;
case NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_LAND:
navigator_enabled = true;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_TERMINATION:
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@@ -1769,21 +1741,6 @@ set_control_mode()
default:
break;
}
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
}
}
void
@@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
src/modules/commander/state_machine_helper.cpp
+88 -334
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <stdio.h>
@@ -59,30 +60,20 @@
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
@@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
@@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
}
bool
check_arming_state_changed()
{
if (arming_state_changed) {
arming_state_changed = false;
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if requested the same state because conditions may be changed */
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
@@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
bool
check_main_state_changed()
{
if (main_state_changed) {
main_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_failsafe_state_changed()
{
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
return false;
}
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
valid_transition = true;
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
@@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown hil state");
warnx("Unknown HIL state");
break;
}
}
if (valid_transition) {
if (ret = TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// XXX also set lockdown here
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Transition from one failsafe state to another
*/
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
* Check failsafe and main status and set navigation status for navigator accordingly
*/
void set_nav_state(struct vehicle_status_s *status)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
}
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
switch (status->failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_MANUAL:
status->set_nav_state = NAVIGATION_STATE_MANUAL;
break;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAVIGATION_STATE_RTL;
ret = TRANSITION_CHANGED;
}
case MAIN_STATE_ALTCTL:
status->set_nav_state = NAVIGATION_STATE_ALTCTL;
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAVIGATION_STATE_LAND;
ret = TRANSITION_CHANGED;
}
case MAIN_STATE_POSCTL:
status->set_nav_state = NAVIGATION_STATE_POSCTL;
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
case MAIN_STATE_AUTO_MISSION:
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
break;
case MAIN_STATE_AUTO_LOITER:
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
break;
case MAIN_STATE_AUTO_RTL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
break;
case MAIN_STATE_ACRO:
status->set_nav_state = NAVIGATION_STATE_ACRO;
break;
default:
break;
}
break;
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
case FAILSAFE_STATE_RTL_RC:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
break;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
case FAILSAFE_STATE_RTL_DL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL;
break;
case FAILSAFE_STATE_LAND:
status->set_nav_state = NAVIGATION_STATE_LAND;
break;
case FAILSAFE_STATE_TERMINATION:
status->set_nav_state = NAVIGATION_STATE_TERMINATION;
break;
default:
break;
}
return ret;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
// */
// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
// *
// * START SUBSYSTEM/EMERGENCY FUNCTIONS
// * */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was removed something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was disabled something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
//
//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
//{
// int ret = 1;
//
//// /* Switch on HIL if in standby and not already in HIL mode */
//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
//// && !current_status->flag_hil_enabled) {
//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
//// /* Enable HIL on request */
//// current_status->flag_hil_enabled = true;
//// ret = OK;
//// state_machine_publish(status_pub, current_status, mavlink_fd);
//// publish_armed_status(current_status);
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
//// } else {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
//// }
//// }
//
// /* switch manual / auto */
// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
// }
//
// /* vehicle is disarmed, mode requests arming */
// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only arm in standby state */
// // XXX REMOVE
// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
// ret = OK;
// printf("[cmd] arming due to command request\n");
// }
// }
//
// /* vehicle is armed, mode requests disarming */
// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[cmd] disarming due to command request\n");
// }
// }
//
// /* NEVER actually switch off HIL without reboot */
// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
// ret = ERROR;
// }
//
// return ret;
//}
src/modules/commander/state_machine_helper.h
+4 -12
View File
@@ -56,25 +56,17 @@ typedef enum {
} transition_result_t;
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed();
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
bool check_main_state_changed();
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool check_failsafe_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
void set_nav_state(struct vehicle_status_s *status);
#endif /* STATE_MACHINE_HELPER_H_ */