diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-08 18:47:46 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-08 18:47:46 +0200 |
commit | 7a912a3fe47bced7c04d948cc3da094fea7542bc (patch) | |
tree | d653955dc344c26eb63550432c75c0589ae82e5f /apps/commander/state_machine_helper.c | |
parent | 7a6a4b93525ea62484a5df02f392b72a519ac248 (diff) | |
download | px4-firmware-7a912a3fe47bced7c04d948cc3da094fea7542bc.tar.gz px4-firmware-7a912a3fe47bced7c04d948cc3da094fea7542bc.tar.bz2 px4-firmware-7a912a3fe47bced7c04d948cc3da094fea7542bc.zip |
Minor but important fixes across system
Diffstat (limited to 'apps/commander/state_machine_helper.c')
-rw-r--r-- | apps/commander/state_machine_helper.c | 73 |
1 files changed, 34 insertions, 39 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a71bb797d..ebae1c61c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -417,6 +417,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status) { + // XXX CHANGE BACK if (current_status->state_machine == SYSTEM_STATE_STANDBY) { printf("[commander] arming\n"); do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); @@ -466,61 +467,55 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode) { - commander_state_machine_t current_system_state = current_status->state_machine; - printf("in update state request\n"); uint8_t ret = 1; - /* Switch on HIL if in standby */ - if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { - /* Enable HIL on request */ - current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; - ret = OK; - state_machine_publish(status_pub, current_status); - printf("[commander] Enabling HIL\n"); - } - - /* NEVER actually switch off HIL without reboot */ - if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); - ret = ERROR; - } + current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; + /* Set manual input enabled flag */ + current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - //TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c) - switch (mode) { - case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED - if (current_system_state == SYSTEM_STATE_STANDBY) { + /* vehicle is disarmed, mode requests arming */ + if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only arm in standby state */ + // XXX REMOVE + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* Set armed flag */ current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; /* Set manual input enabled flag */ current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; + printf("[commander] arming due to command request\n"); } + } - break; - - case MAV_MODE_MANUAL_DISARMED: - if (current_system_state == SYSTEM_STATE_GROUND_READY) { + /* vehicle is armed, mode requests disarming */ + //if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only disarm in ground ready */ + //if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { /* Clear armed flag, leave manual input enabled */ - current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - } - - break; + // current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + // do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + // ret = OK; + // printf("[commander] disarming due to command request\n"); + //} + //} - default: - break; + /* Switch on HIL if in standby */ + if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { + /* Enable HIL on request */ + current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; + ret = OK; + state_machine_publish(status_pub, current_status); + printf("[commander] Enabling HIL\n"); } - #warning STATE MACHINE IS INCOMPLETE HERE - -// if(ret != 0) //Debug -// { -// strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected"); -// global_data_send_mavlink_message_out(&mavlink_statustext_msg_content); -// } + /* NEVER actually switch off HIL without reboot */ + if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { + fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + ret = ERROR; + } return ret; } |