diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-01 16:28:53 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-01 16:28:53 +0100 |
| commit | aa1d57c08549f9c75582e5608b93adc5254b04a4 (patch) | |
| tree | ad648ed9fcdf3d3fc7d283443a329d1a2d17d442 /apps/commander/state_machine_helper.c | |
| parent | fd14084768be3873f502f2897603dd358f7d1770 (diff) | |
| download | px4-firmware-aa1d57c08549f9c75582e5608b93adc5254b04a4.tar.gz px4-firmware-aa1d57c08549f9c75582e5608b93adc5254b04a4.tar.bz2 px4-firmware-aa1d57c08549f9c75582e5608b93adc5254b04a4.zip | |
Allowed mode switching via command
Diffstat (limited to 'apps/commander/state_machine_helper.c')
| -rw-r--r-- | apps/commander/state_machine_helper.c | 35 |
1 files changed, 22 insertions, 13 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index c6b246574..aa916dafa 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -565,6 +565,28 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ { printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + /* 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("[commander] 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) { + mavlink_log_critical(mavlink_fd, "[commander] 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_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { @@ -587,19 +609,6 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ } } - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - /* 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("[commander] 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) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.") - } - /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); |