diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-14 11:19:06 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-14 11:19:06 +0200 |
| commit | 747eda92b9396e38746d505e5d79a7528e117c89 (patch) | |
| tree | c1c50c6922116bab981c852755cff85606643738 /src | |
| parent | 373b1705c19627b97f1c65c1e947802e6b88af83 (diff) | |
| download | px4-firmware-747eda92b9396e38746d505e5d79a7528e117c89.tar.gz px4-firmware-747eda92b9396e38746d505e5d79a7528e117c89.tar.bz2 px4-firmware-747eda92b9396e38746d505e5d79a7528e117c89.zip | |
commander: handle VEHICLE_CMD_NAV_GUIDED_ENABLE
Diffstat (limited to 'src')
| -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 c11286789..e9db923e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -611,7 +611,24 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; - + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + } + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: |