aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander')
-rw-r--r--src/modules/commander/commander.cpp39
-rw-r--r--src/modules/commander/state_machine_helper.cpp58
-rw-r--r--src/modules/commander/state_machine_helper.h4
3 files changed, 49 insertions, 52 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d51bb63ff..7715f73e0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
- transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+ transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
+ if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+ /* recover from failsafe */
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+ if (res == TRANSITION_CHANGED) {
+ mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe");
+ }
+ }
+
/* fill current_status according to mode switches */
check_mode_switches(&sp_man, &status);
@@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
}
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
- transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
if (res == TRANSITION_CHANGED) {
- mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
- status.set_nav_state = NAV_STATE_RTL;
- status.set_nav_state_timestamp = hrt_absolute_time();
- } else if (status.main_state != MAIN_STATE_SEATBELT) {
- res = main_state_transition(&status, MAIN_STATE_SEATBELT);
- if (res == TRANSITION_CHANGED) {
- mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
- }
+ mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
}
+ // TODO add other failsafe modes if position estimate not available
}
}
}
- /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
- if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
- transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
- if (flighttermination_res == TRANSITION_CHANGED) {
+ // TODO remove this hack
+ /* flight termination in manual mode if assisted switch is on easy position */
+ if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+ if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
}
- } else {
- flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
-
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1176,12 +1175,12 @@ 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 flighttermination_state_changed = check_flighttermination_state_changed();
+ bool failsafe_state_changed = check_failsafe_state_changed();
hrt_abstime t1 = hrt_absolute_time();
- if (arming_state_changed || main_state_changed) {
- mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
+ if (arming_state_changed || main_state_changed || failsafe_state_changed) {
+ mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state);
status_changed = true;
}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 731e0e3ff..90ca2a6d2 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -63,7 +63,7 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
-static bool flighttermination_state_changed = true;
+static bool failsafe_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -287,10 +287,10 @@ check_main_state_changed()
}
bool
-check_flighttermination_state_changed()
+check_failsafe_state_changed()
{
- if (flighttermination_state_changed) {
- flighttermination_state_changed = false;
+ if (failsafe_state_changed) {
+ failsafe_state_changed = false;
return true;
} else {
@@ -361,41 +361,39 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/**
-* Transition from one flightermination state to another
+* Transition from one failsafe state to another
*/
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
{
transition_result_t ret = TRANSITION_DENIED;
- /* only check transition if the new state is actually different from the current one */
- if (new_flighttermination_state == status->flighttermination_state) {
- ret = TRANSITION_NOT_CHANGED;
-
- } else {
+ /* only check transition if the new state is actually different from the current one */
+ if (new_failsafe_state == status->failsafe_state) {
+ ret = TRANSITION_NOT_CHANGED;
- switch (new_flighttermination_state) {
- case FLIGHTTERMINATION_STATE_ON:
- ret = TRANSITION_CHANGED;
- status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
- warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
- break;
- case FLIGHTTERMINATION_STATE_OFF:
- ret = TRANSITION_CHANGED;
- status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
- break;
+ } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) {
+ switch (new_failsafe_state) {
+ case FAILSAFE_STATE_NORMAL:
+ ret = TRANSITION_CHANGED;
+ break;
+ case FAILSAFE_STATE_RTL:
+ ret = TRANSITION_CHANGED;
+ break;
+ case FAILSAFE_STATE_TERMINATION:
+ ret = TRANSITION_CHANGED;
+ break;
- default:
- break;
- }
+ default:
+ break;
+ }
- if (ret == TRANSITION_CHANGED) {
- flighttermination_state_changed = true;
- // TODO
- //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
- }
+ if (ret == TRANSITION_CHANGED) {
+ status->failsafe_state = new_failsafe_state;
+ failsafe_state_changed = true;
}
+ }
- return ret;
+ return ret;
}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index e569fb4f3..f04879ff9 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
bool check_main_state_changed();
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
-bool check_flighttermination_state_changed();
+bool check_failsafe_state_changed();
void set_navigation_state_changed();
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-02 22:59:12 +0000