diff options
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -rw-r--r-- | src/modules/commander/commander.cpp | 25 |
1 files changed, 25 insertions, 0 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f56d5e3..b635a22cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s armed_local->force_failsafe = false; warnx("disabling failsafe"); } + /* param2 is currently used for other failsafe modes */ + status_local->engine_failure_cmd = false; + status_local->data_link_lost_cmd = false; + status_local->gps_failure_cmd = false; + status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { + /* reset all commanded failure modes */ + warnx("revert to normal mode"); + } else if ((int)cmd->param2 == 1) { + /* trigger engine failure mode */ + status_local->engine_failure_cmd = true; + warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { + /* trigger data link loss mode */ + status_local->data_link_lost_cmd = true; + warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { + /* trigger gps loss mode */ + status_local->gps_failure_cmd = true; + warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { + /* trigger rc loss mode */ + status_local->rc_signal_lost_cmd = true; + warnx("rc loss mode commanded"); + } cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; |