diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-21 18:10:34 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-21 18:10:34 -0800 |
commit | c056410f8439e760905cb50fe08c49c3a0b344e5 (patch) | |
tree | ab9358dee0090da2c3af2d4c85d68f78cdc99cc8 /apps | |
parent | ebe0285ce7964ac1a81a65bae417e978cf366466 (diff) | |
download | px4-firmware-c056410f8439e760905cb50fe08c49c3a0b344e5.tar.gz px4-firmware-c056410f8439e760905cb50fe08c49c3a0b344e5.tar.bz2 px4-firmware-c056410f8439e760905cb50fe08c49c3a0b344e5.zip |
Checkpoint: Added arming check function
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/state_machine_helper.c | 55 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 2 |
2 files changed, 57 insertions, 0 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e6344f1a8..0611bb52c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,6 +254,61 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state != current_arming_state) { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_arming_state == ARMING_STATE_STANDBY) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = OK; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + } + break; + default: + break; + } + } + return ret; +} + + + /* * This functions does not evaluate any input flags but only checks if the transitions * are valid. diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 1c0564d07..cf1fa80cd 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,4 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); + #endif /* STATE_MACHINE_HELPER_H_ */ |