From 0fe5aeb02c6ab0ac9c50f54d028cd5dde908e051 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 10:39:57 -0700 Subject: Checkpoint: More state machine fixes, commited to wrong branch and now copied over --- apps/commander/commander.c | 32 ++++++++++++++++++++++++-------- apps/commander/state_machine_helper.c | 34 ++++++++++++++++++++++++---------- 2 files changed, 48 insertions(+), 18 deletions(-) (limited to 'apps/commander') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2fdcf4ce3..a3e8fb745 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -60,13 +60,9 @@ #include #include #include -#include -#include -#include -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include #include + #include #include #include @@ -80,11 +76,19 @@ #include #include #include -#include +#include +#include +#include + +#include #include #include #include +#include + +#include "state_machine_helper.h" + /* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include @@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -120,6 +124,8 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; @@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; current_status.flag_fmu_armed = false; current_status.flag_io_armed = false; // XXX read this from somewhere @@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[]) // current_status.state_machine == SYSTEM_STATE_AUTO || // current_status.state_machine == SYSTEM_STATE_MANUAL)) { // /* armed */ -// led_toggle(LED_BLUE); + led_toggle(LED_BLUE); } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not armed */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 742b7fe07..79394e2ba 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,19 +40,20 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include #include "state_machine_helper.h" - 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; @@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; case ARMING_STATE_STANDBY: @@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; default: @@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = false; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } break; @@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } } break; @@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } break; @@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } break; @@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //void publish_armed_status(const struct vehicle_status_s *current_status) //{ // struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; +// armed.armed = current_status->flag_fmu_armed; // /* lock down actuators if required, only in HIL */ // armed.lockdown = (current_status->flag_hil_enabled) ? true : false; // orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); //// //// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_system_armed) { +//// current_status->flag_fmu_armed) { //// //// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") //// @@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (!(current_status->flag_fmu_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) { @@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only disarm in ground ready */ // if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { // do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -- cgit v1.2.3