aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/state_machine_helper.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r--src/modules/commander/state_machine_helper.cpp414
1 files changed, 84 insertions, 330 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 6f0e9794a..214480079 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -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 MAIN_STATE_POSCTL:
+ status->set_nav_state = NAVIGATION_STATE_POSCTL;
break;
- case FAILSAFE_STATE_LAND:
+ case MAIN_STATE_AUTO_MISSION:
+ status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
+ break;
- /* 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_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 FAILSAFE_STATE_TERMINATION:
- /* always allowed */
- ret = TRANSITION_CHANGED;
+ 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;
-
- } else {
- ret = TRANSITION_NOT_CHANGED;
- }
- }
- }
+ case FAILSAFE_STATE_RTL_RC:
+ status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
+ break;
- return ret;
-}
+ 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;
-// /*
-// * 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;
-//}
+ default:
+ break;
+ }
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 13:53:04 +0000