From be7aeb754b85016e2609508d1c059797d5068ec1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 20:01:54 -0800 Subject: Checkpoint: Added flag checks inside arming state update --- apps/commander/state_machine_helper.c | 35 +++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) (limited to 'apps/commander/state_machine_helper.c') diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 0611bb52c..e5ef00e93 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,49 +254,60 @@ 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 check_arming_state_transition(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_arming_state) { + if (new_arming_state != current_state->arming_state) { switch (new_arming_state) { case ARMING_STATE_INIT: /* allow going back from INIT for calibration */ - if (current_arming_state == ARMING_STATE_STANDBY) { + if (current_state->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; + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->flag_system_sensors_initialized) { + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } 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) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ ret = OK; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ - if (current_arming_state == ARMING_STATE_ARMED) { + if (current_state->arming_state == ARMING_STATE_ARMED) { ret = OK; + + /* XXX conditions for an error state? */ } 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) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; } break; -- cgit v1.2.3