diff options
| author | Julian Oes <joes@student.ethz.ch> | 2013-02-22 15:52:13 -0800 |
|---|---|---|
| committer | Julian Oes <joes@student.ethz.ch> | 2013-02-22 15:52:13 -0800 |
| commit | 793b482e00013ea66bb1b0cdc0366bb720648938 (patch) | |
| tree | 41e64986c23ddca981f51a0abdd70dd0abdb2562 /apps/commander/state_machine_helper.c | |
| parent | cbfa64b59eef8362494d0753ce3567e804f2d682 (diff) | |
| download | px4-firmware-793b482e00013ea66bb1b0cdc0366bb720648938.tar.gz px4-firmware-793b482e00013ea66bb1b0cdc0366bb720648938.tar.bz2 px4-firmware-793b482e00013ea66bb1b0cdc0366bb720648938.zip | |
Checkpoint: Navigation states and arming seem to work
Diffstat (limited to 'apps/commander/state_machine_helper.c')
| -rw-r--r-- | apps/commander/state_machine_helper.c | 25 |
1 files changed, 19 insertions, 6 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 6c15bd725..ae7f2a1c1 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -255,10 +255,13 @@ //} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_state->arming_state) { + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { switch (new_arming_state) { case ARMING_STATE_INIT: @@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta default: break; } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + return ret; } @@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_state->navigation_state) { + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: @@ -561,13 +572,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current default: break; } - } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + + return ret; } |