fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 07:40:34 +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/drivers/blinkm/blinkm.cpp
+3 -2
View File
@@ -655,13 +655,14 @@ BlinkM::led()
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
src/drivers/gps/mtk.cpp
+2 -2
View File
@@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->lon = 0;
// Indicate this data is not usable and bail out
_gps_position->eph_m = 1000.0f;
_gps_position->epv_m = 1000.0f;
_gps_position->eph = 1000.0f;
_gps_position->epv = 1000.0f;
_gps_position->fix_type = 0;
return;
}
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_ */
src/modules/mavlink/mavlink_messages.cpp
+53 -33
View File
@@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
switch (status->set_nav_state) {
case NAVIGATION_STATE_MANUAL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
} else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_POSCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (status->main_state == MAIN_STATE_ACRO) {
case NAVIGATION_STATE_ACRO:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
}
break;
} else {
/* use navigation state when navigator is active */
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
case NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
case NAVIGATION_STATE_POSCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
case NAVIGATION_STATE_AUTO_MISSION:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
case NAVIGATION_STATE_AUTO_LOITER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
case NAVIGATION_STATE_LAND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
break;
case NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
}
*mavlink_custom_mode = custom_mode.data;
src/modules/navigator/navigator_main.cpp
+7 -233
View File
@@ -32,7 +32,6 @@
****************************************************************************/
/**
* @file navigator_main.cpp
* Implementation of the main navigation state machine.
*
* Handles mission items, geo fencing and failsafe navigation behavior.
* Published the position setpoint triplet for the position controller.
@@ -75,7 +74,6 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/state_table.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <geo/geo.h>
@@ -102,7 +100,7 @@ static const int ERROR = -1;
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
class Navigator : public StateTable
class Navigator
{
public:
/**
@@ -116,7 +114,7 @@ public:
~Navigator();
/**
* Start the navigator task.
* Start the navigator task.
*
* @return OK on success.
*/
@@ -200,24 +198,6 @@ private:
param_t takeoff_alt;
} _parameter_handles; /**< handles for parameters */
enum Event {
EVENT_NONE_REQUESTED,
EVENT_LOITER_REQUESTED,
EVENT_MISSION_REQUESTED,
EVENT_RTL_REQUESTED,
EVENT_TAKEN_OFF,
EVENT_LANDED,
EVENT_MISSION_CHANGED,
EVENT_HOME_POSITION_CHANGED,
EVENT_MISSION_ITEM_REACHED,
MAX_EVENT
}; /**< possible events that can be thrown at state machine */
/**
* State machine transition table
*/
static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
/**
* Update our local parameter cache.
*/
@@ -268,41 +248,6 @@ private:
*/
void task_main();
/**
* Functions that are triggered when a new state is entered.
*/
bool start_none_on_ground();
bool start_none_in_air();
bool start_auto_on_ground();
bool start_loiter();
bool start_mission();
bool advance_mission();
bool start_rtl();
bool advance_rtl();
bool start_land();
/**
* Reset all "mission item reached" flags.
*/
void reset_reached();
/**
* Check if current mission item has been reached.
*/
bool check_mission_item_reached();
/**
* Set mission items by accessing the mission class.
* If failing, tell the state machine to loiter.
*/
bool set_mission_items();
/**
* Set a RTL item by accessing the RTL class.
* If failing, tell the state machine to loiter.
*/
void set_rtl_item();
/**
* Translate mission item to a position setpoint.
*/
@@ -329,7 +274,6 @@ Navigator *g_navigator;
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
_task_should_exit(false),
_navigator_task(-1),
_mavlink_fd(-1),
@@ -368,11 +312,6 @@ Navigator::Navigator() :
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
/* Initialize state machine */
myState = NAV_STATE_NONE_ON_GROUND;
start_none_on_ground();
}
Navigator::~Navigator()
@@ -615,41 +554,6 @@ Navigator::task_main()
/* vehicle status updated */
if (fds[6].revents & POLLIN) {
vehicle_status_update();
/* commander requested new navigation mode, forward it to state machine */
switch (_vstatus.set_nav_state) {
case NAVIGATION_STATE_NONE:
dispatch(EVENT_NONE_REQUESTED);
break;
case NAVIGATION_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAVIGATION_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED);
break;
case NAVIGATION_STATE_RTL:
dispatch(EVENT_RTL_REQUESTED);
break;
case NAVIGATION_STATE_LAND:
/* TODO: add and test this */
// dispatch(EVENT_LAND_REQUESTED);
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
/* commander sets in-air/on-ground flag as well */
if (_vstatus.condition_landed) {
dispatch(EVENT_LANDED);
} else {
dispatch(EVENT_TAKEN_OFF);
}
}
/* parameters updated */
@@ -666,30 +570,21 @@ Navigator::task_main()
/* offboard mission updated */
if (fds[4].revents & POLLIN) {
offboard_mission_update();
/* TODO: check if mission really changed */
dispatch(EVENT_MISSION_CHANGED);
}
/* onboard mission updated */
if (fds[5].revents & POLLIN) {
onboard_mission_update();
/* TODO: check if mission really changed */
dispatch(EVENT_MISSION_CHANGED);
}
/* home position updated */
if (fds[2].revents & POLLIN) {
home_position_update();
/* TODO check if home position really changed */
dispatch(EVENT_HOME_POSITION_CHANGED);
}
/* global position updated */
if (fds[1].revents & POLLIN) {
global_position_update();
if (check_mission_item_reached()) {
dispatch(EVENT_MISSION_ITEM_REACHED);
}
/* Check geofence violation */
if (!_geofence.inside(&_global_pos)) {
@@ -765,125 +660,8 @@ Navigator::status()
} else {
warnx("Geofence not set");
}
switch (myState) {
case NAV_STATE_NONE_ON_GROUND:
warnx("State: None on ground");
break;
case NAV_STATE_NONE_IN_AIR:
warnx("State: None in air");
break;
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case NAV_STATE_RTL:
warnx("State: RTL");
break;
case NAV_STATE_LAND:
warnx("State: Land");
break;
default:
warnx("State: Unknown");
break;
}
}
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* NAV_STATE_NONE_ON_GROUND */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
/* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
/* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND},
},
{
/* NAV_STATE_NONE_IN_AIR */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR},
/* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR},
/* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR},
},
{
/* NAV_STATE_AUTO_ON_GROUND */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
/* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
},
{
/* NAV_STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* NAV_STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION},
},
{
/* NAV_STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL},
},
{
/* NAV_STATE_LAND */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND},
/* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND},
},
};
#if 0
bool
Navigator::start_none_on_ground()
{
@@ -984,13 +762,11 @@ Navigator::set_mission_items()
/* if we fail to set the current mission, continue to loiter */
if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
dispatch(EVENT_LOITER_REQUESTED);
return false;
}
/* if we got an RTL mission item, switch to RTL mode and give up */
if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
dispatch(EVENT_RTL_REQUESTED);
return false;
}
@@ -1042,7 +818,6 @@ Navigator::start_rtl()
}
/* if RTL doesn't work, fallback to loiter */
dispatch(EVENT_LOITER_REQUESTED);
return false;
}
@@ -1066,7 +841,6 @@ Navigator::advance_rtl()
return true;
}
dispatch(EVENT_LOITER_REQUESTED);
return false;
}
@@ -1105,7 +879,7 @@ Navigator::start_land()
_update_triplet = true;
return true;
}
#endif
void
Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
{
@@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio
sp->type = SETPOINT_TYPE_NORMAL;
}
}
#if 0
bool
Navigator::check_mission_item_reached()
{
@@ -1251,12 +1025,12 @@ Navigator::reset_reached()
_waypoint_yaw_reached = false;
}
#endif
void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
/* TODO: set nav_state */
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
src/modules/systemlib/state_table.h
+14 -14
View File
@@ -33,7 +33,7 @@
/**
* @file state_table.h
*
*
* Finite-State-Machine helper class for state table
* @author: Julian Oes <julian@oes.ch>
*/
@@ -44,32 +44,32 @@
class StateTable
{
public:
typedef bool (StateTable::*Action)();
typedef void (StateTable::*Action)();
struct Tran {
Action action;
unsigned nextState;
};
StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
#define NO_ACTION &StateTable::doNothing
#define ACTION(_target) StateTable::Action(_target)
virtual ~StateTable() {}
void dispatch(unsigned const sig) {
/* get transition using state table */
Tran const *t = myTable + myState*myNsignals + sig;
/* first up change state, this allows to do further dispatchs in the state functions */
/* now execute state function, if it runs with success, accept new state */
if ((this->*(t->action))()) {
myState = t->nextState;
}
/* accept new state */
myState = t->nextState;
/* */
(this->*(t->action))();
}
bool doNothing() {
return true;
void doNothing() {
return;
}
protected:
unsigned myState;
@@ -79,4 +79,4 @@ private:
unsigned myNstates;
};
#endif
#endif
src/modules/uORB/topics/mission_result.h
+1
View File
@@ -56,6 +56,7 @@ struct mission_result_s
bool mission_reached; /**< true if mission has been reached */
unsigned mission_index_reached; /**< index of the mission which has been reached */
unsigned index_current_mission; /**< index of the current mission */
bool mission_finished; /**< true if mission has been completed */
};
/**
src/modules/uORB/topics/position_setpoint_triplet.h
+7 -8
View File
@@ -74,14 +74,13 @@ struct position_setpoint_s
};
typedef enum {
NAV_STATE_NONE_ON_GROUND = 0,
NAV_STATE_NONE_IN_AIR,
NAV_STATE_AUTO_ON_GROUND,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
NAV_STATE_MANUAL = 0,
NAV_STATE_POSCTL,
NAV_STATE_AUTO,
NAV_STATE_RC_LOSS,
NAV_STATE_DL_LOSS,
NAV_STATE_TERMINATION,
MAX_NAV_STATE
} nav_state_t;
/**
src/modules/uORB/topics/vehicle_status.h
+22 -13
View File
@@ -63,9 +63,11 @@ typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_MAX
MAIN_STATE_MAX,
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -78,7 +80,7 @@ typedef enum {
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX
ARMING_STATE_MAX,
} arming_state_t;
typedef enum {
@@ -88,18 +90,25 @@ typedef enum {
typedef enum {
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */
FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */
FAILSAFE_STATE_LAND, /**< Land as safe as possible */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
FAILSAFE_STATE_MAX,
} failsafe_state_t;
typedef enum {
NAVIGATION_STATE_NONE = 0,
NAVIGATION_STATE_MISSION,
NAVIGATION_STATE_LOITER,
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_LAND
NAVIGATION_STATE_MANUAL = 0,
NAVIGATION_STATE_ACRO,
NAVIGATION_STATE_ALTCTL,
NAVIGATION_STATE_POSCTL,
NAVIGATION_STATE_AUTO_MISSION,
NAVIGATION_STATE_AUTO_LOITER,
NAVIGATION_STATE_AUTO_RTL,
NAVIGATION_STATE_AUTO_RTL_RC,
NAVIGATION_STATE_AUTO_RTL_DL,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_TERMINATION,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
@@ -160,10 +169,10 @@ struct vehicle_status_s {
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
main_state_t main_state; /**< main state machine */
navigation_state_t set_nav_state; /**< set navigation state machine to specified value */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
hil_state_t hil_state; /**< current hil state */
failsafe_state_t failsafe_state; /**< current failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */