diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-28 23:12:16 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-28 23:12:16 +0400 |
commit | 8c1067a017714394955892e3159c8e0c61bd4ba1 (patch) | |
tree | e95574e6fdee3e6c299c2d347c181a98bc8fddb0 /src/modules/commander/state_machine_helper.cpp | |
parent | 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 (diff) | |
download | px4-firmware-8c1067a017714394955892e3159c8e0c61bd4ba1.tar.gz px4-firmware-8c1067a017714394955892e3159c8e0c61bd4ba1.tar.bz2 px4-firmware-8c1067a017714394955892e3159c8e0c61bd4ba1.zip |
State machine rewritten, compiles, WIP
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 797 |
1 files changed, 319 insertions, 478 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4a7aa66b7..043ccfd0c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,102 +62,110 @@ #endif static const int ERROR = -1; -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; +transition_result_t +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + } else { switch (new_arming_state) { - case ARMING_STATE_INIT: + case ARMING_STATE_INIT: - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - 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->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + break; - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after 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; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: + case ARMING_STATE_STANDBY: - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { - ret = OK; + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; armed->armed = false; - armed->ready_to_arm = false; + armed->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } - break; - case ARMING_STATE_IN_AIR_RESTORE: + } - /* XXX implement */ - break; - default: - break; - } + break; + + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + case ARMING_STATE_STANDBY_ERROR: + + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + hrt_abstime t = hrt_absolute_time(); + status->arming_state = new_arming_state; + armed->timestamp = t; } } @@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta } - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + } else { - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: - - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_MANUAL_STANDBY: - - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - case NAVIGATION_STATE_MANUAL: + case MAIN_STATE_SEATBELT: - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; - - case NAVIGATION_STATE_ASSISTED_STANDBY: - - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { - - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SEATBELT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SIMPLE: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + /* need position estimate */ + // TODO only need altitude estimate really + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_AUTO_TAKEOFF: + } else { + ret = TRANSITION_CHANGED; + } - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + break; - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + case MAIN_STATE_EASY: - default: - break; - } + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; + + case MAIN_STATE_AUTO: + + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } + + break; + } - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; } } - + return ret; +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_STANDBY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + current_status->navigation_state = new_navigation_state; + } + } return ret; } @@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s switch (new_state) { - case HIL_STATE_OFF: + case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } - case HIL_STATE_ON: + break; - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + case HIL_STATE_ON: - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } - default: - warnx("Unknown hil state"); - break; + break; + + default: + warnx("Unknown hil state"); + break; } } @@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); ret = OK; + } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } |