diff options
| author | Julian Oes <joes@student.ethz.ch> | 2013-02-17 15:04:01 -0800 |
|---|---|---|
| committer | Julian Oes <joes@student.ethz.ch> | 2013-02-17 15:04:01 -0800 |
| commit | 3bc385c789f2b39cda066551ff1d5b767ab26aec (patch) | |
| tree | f6912e76eb408546cb8b808f08d7f9051e8b8da9 /apps/commander | |
| parent | 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba (diff) | |
| download | px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.tar.gz px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.tar.bz2 px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.zip | |
Checkpoint: Added arming state check
Diffstat (limited to 'apps/commander')
| -rw-r--r-- | apps/commander/commander.c | 6 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 22 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.h | 2 |
3 files changed, 29 insertions, 1 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8716caef7..1bfdd5660 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } + /* make changes in state machine if needed */ + update_state_machine(stat_pub, ¤t_status, mavlink_fd); + + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1ec73110..d89be781a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,6 +53,28 @@ #include "state_machine_helper.h" +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* check arming first */ + if (current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); + } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (current_status->flag_system_sensors_ok) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } + + /* now determine the navigation state */ +} + /** * Transition from one navigation state to another */ diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf..ed18bfbd2 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,6 +47,8 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); |