diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-17 23:07:07 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-17 23:07:07 -0800 |
commit | 47b05eeb87191fd0b380de008299f85262bc8953 (patch) | |
tree | d37580e6ed72f1569564882c0bda4d3b8582ba08 | |
parent | 3bc385c789f2b39cda066551ff1d5b767ab26aec (diff) | |
download | px4-firmware-47b05eeb87191fd0b380de008299f85262bc8953.tar.gz px4-firmware-47b05eeb87191fd0b380de008299f85262bc8953.tar.bz2 px4-firmware-47b05eeb87191fd0b380de008299f85262bc8953.zip |
Checkpoint, arming/disarming still has a bug
-rw-r--r-- | apps/commander/commander.c | 225 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 272 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 15 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 34 |
5 files changed, 304 insertions, 244 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1bfdd5660..4e470f1d9 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } /* request to disarm */ // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } } } break; @@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param1) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "starting gyro cal"); +// tune_confirm(); +// do_gyro_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished gyro cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param2) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting mag cal"); +// tune_confirm(); +// do_mag_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished mag cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - // XXX implement this - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// // XXX implement this +// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting trim cal"); +// tune_confirm(); +// do_rc_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished trim cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); +// tune_confirm(); +// do_accel_calibration(status_pub, ¤t_status); +// tune_confirm(); +// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -1315,7 +1322,7 @@ 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.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; @@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + // XXX for now just set sensors as initialized + current_status.flag_system_sensors_initialized = true; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = false; - stick_on_counter = 0; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + stick_off_counter = 0; } else { stick_off_counter++; @@ -1898,7 +1908,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) { - current_status.flag_system_armed = true; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { @@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } - /* make changes in state machine if needed */ - update_state_machine(stat_pub, ¤t_status, mavlink_fd); - /* Store old modes to detect and act on state transitions */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index d89be781a..a0b786aab 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,33 +53,12 @@ #include "state_machine_helper.h" -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* check arming first */ - if (current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); - } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (current_status->flag_system_sensors_ok) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } - - /* now determine the navigation state */ -} - /** * Transition from one navigation state to another */ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { + bool valid_path = false; bool valid_transition = false; int ret = ERROR; @@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else { switch (new_state) { - case NAVIGATION_STATE_INIT: - - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); - valid_transition = true; - } - break; - case NAVIGATION_STATE_STANDBY: - if (current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->navigation_state == NAVIGATION_STATE_MANUAL + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); + } + valid_path = true; } break; case NAVIGATION_STATE_MANUAL: - if ( - ( current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) - && current_status->arming_state == ARMING_STATE_ARMED) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + /* only check for armed flag when coming from STANDBY XXX does that make sense? */ + if (current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); valid_transition = true; + valid_path = true; } break; @@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ || current_status->navigation_state == NAVIGATION_STATE_LOITER || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); valid_transition = true; + valid_path = true; } break; case NAVIGATION_STATE_LOITER: - if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) - && current_status->flag_global_position_valid) - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + /* Check for position lock when coming from MANUAL or SEATBELT */ + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + + } else { + mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); valid_transition = true; - } + valid_path = true; + } break; case NAVIGATION_STATE_AUTO_READY: - if ( - (current_status->navigation_state == NAVIGATION_STATE_STANDBY - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - || current_status->navigation_state == NAVIGATION_STATE_LAND) { + /* coming from STANDBY pos and home lock are needed */ + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); - valid_transition = true; + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + } + valid_path = true; + /* coming from LAND home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { + + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_MISSION: - if ( - current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + /* coming from TAKEOFF or RTL is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_RTL: - if ( - current_status->navigation_state == NAVIGATION_STATE_MISSION - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + /* coming from MISSION is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } + valid_path = true; } break; @@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + /* TAKEOFF is straight forward from AUTO READY */ + mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); valid_transition = true; + valid_path = true; } break; @@ -210,27 +258,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_RTL || current_status->navigation_state == NAVIGATION_STATE_LOITER) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); - valid_transition = true; - } - break; - - case NAVIGATION_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->flag_hil_enabled) { + mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + valid_path = true; } - break; default: @@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ state_machine_publish(status_pub, current_status, mavlink_fd); // publish_armed_status(current_status); ret = OK; - } else { + } + + if (!valid_path){ mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } @@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_INIT: if (current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + + mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); valid_transition = true; } break; @@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat // XXX check if coming from reboot? if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); - valid_transition = true; + if (current_status->flag_system_sensors_initialized) { + 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."); + } } break; @@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } break; - case ARMING_STATE_MISSION_ABORT: + case ARMING_STATE_ABORT: if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); valid_transition = true; } break; @@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_ERROR: if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_ABORT || current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); valid_transition = true; } break; @@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_ERROR) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); valid_transition = true; - // XXX reboot here? + 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"); + mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); valid_transition = true; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index ed18bfbd2..bf9296caf 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,8 +47,6 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4636ee36e..e25f1be27 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_LOITER - || v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + } else if (v_status.arming_state == ARMING_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index ba9b9793b..874cf256c 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,8 +60,7 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, NAVIGATION_STATE_LOITER, @@ -70,15 +69,13 @@ typedef enum { NAVIGATION_STATE_RTL, NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, - NAVIGATION_STATE_GROUND_ERROR, - NAVIGATION_STATE_REBOOT } navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, ARMING_STATE_ARMED, - ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ABORT, ARMING_STATE_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE @@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; + //enum VEHICLE_FLIGHT_MODE { // VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ // VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ @@ -168,9 +181,18 @@ struct vehicle_status_s /* system flags - these represent the state predicates */ + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; - bool flag_system_sensors_ok; + bool flag_system_in_air_restore; /**< true if we can restore in mid air */ + bool flag_system_sensors_initialized; + bool flag_system_arming_requested; + bool flag_system_disarming_requested; + bool flag_system_reboot_requested; + bool flag_system_on_ground; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ |