diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-08 16:52:41 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-08 16:52:41 +0100 |
commit | 278e05e425f6aca75e2d6b43a17945b095176997 (patch) | |
tree | a34782d0ca8f528d589ec18b3f037c5a84242354 /src/modules/commander/commander.cpp | |
parent | 4d846b480c9118090fe60a887fb1eb0824b38f56 (diff) | |
download | px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.tar.gz px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.tar.bz2 px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.zip |
added simple flight termination state machine which enbales parachute on request
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 19 |
1 files changed, 18 insertions, 1 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfd4d2f73..40562a4e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + /* Flight termination */ + 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, control_mode); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* reject parachute depoyment not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } + break; + default: break; } @@ -1199,6 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); + bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); @@ -1725,7 +1741,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) continue; /* only handle low-priority commands here */ |