From 36f9f8082e1ac676994cab0a6ee2c7a8344b0216 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 09:32:49 -0800 Subject: Checkpoint: Added flag checks inside navigation state update --- apps/commander/commander.c | 40 +- apps/commander/state_machine_helper.c | 856 ++++++++++++++++------------------ apps/commander/state_machine_helper.h | 8 +- apps/uORB/topics/vehicle_status.h | 1 + 4 files changed, 435 insertions(+), 470 deletions(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4e2b4907b..953ba7a1e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -804,7 +804,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -902,8 +902,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -924,8 +924,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -995,8 +995,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_accel_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX what if this fails + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + //XXX what if this fails + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1857,7 +1858,8 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - navigation_state_update(stat_pub, ¤t_status, mavlink_fd); + #warning do that + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { @@ -1880,7 +1882,7 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; } else { @@ -1892,7 +1894,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e5ef00e93..6c15bd725 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,206 +56,205 @@ /** * Transition from one sytem state to another */ -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int ret = ERROR; - navigation_state_t new_navigation_state; - - /* do the navigation state update depending on the arming state */ - switch (current_status->arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_NONE) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_RETURN) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - /* descent not possible without baro information, fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept LOITER if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_NONE) { - - if (current_status->flag_global_position_valid) { - new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; - } else { - mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Accept MISSION if there is a global position estimate and home position */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_MISSION) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Go to RTL or land if global position estimate and home position is available */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_RETURN - && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - - /* after RTL go to LAND */ - if (current_status->flag_system_returned_to_home) { - new_navigation_state = NAVIGATION_STATE_AUTO_LAND; - } else { - new_navigation_state = NAVIGATION_STATE_AUTO_RTL; - } - - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - } - break; - - case ARMING_STATE_ARMED_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - - - - /* Update the system state in case it has changed */ - if (current_status->navigation_state != new_navigation_state) { - - /* Check if the transition is valid */ - if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { - current_status->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "System state transition not valid"); - } - } - - return; -} - -int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// int ret = ERROR; +// navigation_state_t new_navigation_state; +// +// /* do the navigation state update depending on the arming state */ +// switch (current_status->arming_state) { +// +// /* evaluate the navigation state when disarmed */ +// case ARMING_STATE_STANDBY: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// +// /* Accept AUTO if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); +// +// /* otherwise fallback to seatbelt or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// } +// } +// +// break; +// +// /* evaluate the navigation state when armed */ +// case ARMING_STATE_ARMED: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_NONE) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_RETURN) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// /* descent not possible without baro information, fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept LOITER if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_NONE) { +// +// if (current_status->flag_global_position_valid) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Accept MISSION if there is a global position estimate and home position */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_MISSION) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Go to RTL or land if global position estimate and home position is available */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_RETURN +// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// +// /* after RTL go to LAND */ +// if (current_status->flag_system_returned_to_home) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; +// } else { +// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; +// } +// +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// } +// break; +// +// case ARMING_STATE_ARMED_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_STANDBY_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_REBOOT: +// +// // XXX I don't think we should end up here +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// // XXX not sure what to do here +// break; +// default: +// break; +// } +// +// +// +// /* Update the system state in case it has changed */ +// if (current_status->navigation_state != new_navigation_state) { +// +// /* Check if the transition is valid */ +// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { +// current_status->navigation_state = new_navigation_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// } else { +// mavlink_log_critical(mavlink_fd, "System state transition not valid"); +// } +// } +// +// return; +//} +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 */ @@ -324,20 +323,20 @@ int check_arming_state_transition(struct vehicle_status_s *current_state, arming * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_navigation_state) { + if (new_navigation_state != current_state->navigation_state) { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } @@ -346,93 +345,139 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_MANUAL_STANDBY: /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_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"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_MANUAL: - /* all transitions possible */ - ret = OK; + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + } break; case NAVIGATION_STATE_SEATBELT_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - - ret = OK; + 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_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_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"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_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"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || 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"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_STANDBY: /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_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"); + } else if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_READY: /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - ret = OK; + // 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"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } @@ -441,49 +486,75 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_AUTO_LOITER: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + 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_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_MISSION: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + 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_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state->flag_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_RTL: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + 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_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_LAND: /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - ret = OK; + 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->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + } } break; @@ -492,182 +563,71 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat } } + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + return ret; } -///** -// * Transition from one arming state to another -// */ -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); -// -// if (current_status->arming_state == new_state) { -// warnx("Arming state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case ARMING_STATE_INIT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY) { -// -// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_STANDBY: -// -// // TODO check for sensors -// // XXX check if coming from reboot? -// if (current_status->arming_state == ARMING_STATE_INIT) { -// -// if (current_status->flag_system_sensors_initialized) { -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } else { -// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); -// } -// -// } else if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ARMED: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { -// current_status->flag_system_armed = true; -// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ABORT: -// -// if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ERROR: -// -// if (current_status->arming_state == ARMING_STATE_ARMED -// || current_status->arming_state == ARMING_STATE_ABORT -// || current_status->arming_state == ARMING_STATE_INIT) { -// -// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_REBOOT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_ERROR) { -// -// valid_transition = true; -// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); -// usleep(500000); -// up_systemreset(); -// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -// } -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// if (current_status->arming_state == ARMING_STATE_INIT) { -// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); -// valid_transition = true; -// } -// break; -// default: -// warnx("Unknown arming state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->arming_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); -// } -// -// return ret; -//} -///** -// * Transition from one hil state to another -// */ -//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); -// -// if (current_status->hil_state == new_state) { -// warnx("Hil state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case HIL_STATE_OFF: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = false; -// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); -// valid_transition = true; -// } -// break; -// -// case HIL_STATE_ON: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = true; -// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); -// valid_transition = true; -// } -// break; -// -// default: -// warnx("Unknown hil state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->hil_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); -// } -// -// return ret; -//} +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index f2928db09..5b57cffb7 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -53,7 +53,9 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); -#endif /* STATE_MACHINE_HELPER_H_ */ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); + +#endif /* STATE_MACHINE_HELPER_H_ */ \ No newline at end of file diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 29baff34b..495542244 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -193,6 +193,7 @@ struct vehicle_status_s bool flag_system_reboot_requested; bool flag_system_returned_to_home; + bool flag_auto_mission_available; bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ -- cgit v1.2.3