diff options
| -rw-r--r-- | Documentation/flight_mode_state_machine.odg | bin | 18105 -> 24059 bytes | |||
| -rw-r--r-- | apps/commander/commander.c | 131 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 614 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.h | 4 | ||||
| -rw-r--r-- | apps/uORB/topics/vehicle_status.h | 89 |
5 files changed, 367 insertions, 471 deletions
diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg Binary files differindex b630ecb40..a50366bcb 100644 --- a/Documentation/flight_mode_state_machine.odg +++ b/Documentation/flight_mode_state_machine.odg diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b026f287..8b9e7c49c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); - - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, fall back to m */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.flag_control_position_enabled = true; - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set seatbelt/simple control */ - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); + + /* no valid stick position, go to default */ + + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + + } else { + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if land/RTL is requested - */ + * Check if land/RTL is requested + */ if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.flag_land_requested = false; + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.flag_land_requested = true; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { /* top switch position */ - current_status.flag_mission_activated = true; + current_status.mission_switch = MISSION_SWITCH_MISSION; } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { /* bottom switch position */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else { /* center switch position, set default */ - current_status.flag_mission_activated = false; // XXX default? + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + if (current_status.arming_state == ARMING_STATE_ARMED) { + + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); + } + } } /* handle the case where RC signal was regained */ @@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ -// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || -// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || -// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) -// ) && + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + // ) && 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); @@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aae119d35..2e4935471 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -54,422 +54,330 @@ /** - * Transition from one navigation state to another + * Transition from one sytem 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) +void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_path = false; - bool valid_transition = false; int ret = ERROR; + system_state_t new_system_state; - if (current_status->navigation_state == new_state) { - warnx("Navigation state not changed"); - } else { - - switch (new_state) { - case NAVIGATION_STATE_STANDBY: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - 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) { - - /* 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; - - case NAVIGATION_STATE_SEATBELT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_DESCENT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LOITER: - - /* 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: - - /* coming from STANDBY pos and home lock are needed */ - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + /* + * Firstly evaluate mode switch position + * */ - } else if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_system_state = SYSTEM_STATE_MANUAL; - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - 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, "Rejected AUTO READY state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MISSION: - - /* 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) { - 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"); - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - - } - 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: - - /* 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) { - 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, "Switched to RTL state"); - valid_transition = true; - } - 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; + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL); + } - case NAVIGATION_STATE_TAKEOFF: + /* 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_system_state = SYSTEM_STATE_SEATBELT); + } 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_system_state = SYSTEM_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL; + } + } + } - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + /* + * Next up, check for + */ - /* TAKEOFF is straight forward from AUTO READY */ - mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_LAND: - if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LAND state"); - valid_transition = true; - valid_path = true; - } - break; + /* Update the system state in case it has changed */ + if (current_status->system_state != new_system_state) { - default: - warnx("Unknown navigation state"); - break; + /* Check if the transition is valid */ + if (system_state_update(current_status->system_state, new_system_state) == OK) { + current_status->system_state = new_system_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + } else { + mavlink_log_critical(mavlink_fd, "System state transition not valid"); } } - if (valid_transition) { - current_status->navigation_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } - - if (!valid_path){ - mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); - } - - return ret; + return; } -/** - * Transition from one arming state to another +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. */ -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); +int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { - if (current_status->arming_state == new_state) { - warnx("Arming state not changed"); - valid_transition = true; - - } else { + int ret = ERROR; - switch (new_state) { + /* only check transition if the new state is actually different from the current one */ + if (new_system_state != current_system_state) { - case ARMING_STATE_INIT: + switch (new_system_state) { + case SYSTEM_STATE_INIT: - if (current_status->arming_state == ARMING_STATE_STANDBY) { + /* transitions back to INIT are possible for calibration */ + if (current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); - valid_transition = true; + ret = OK; } - break; - case ARMING_STATE_STANDBY: - - // TODO check for sensors - // XXX check if coming from reboot? - if (current_status->arming_state == ARMING_STATE_INIT) { + break; - 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."); - } + case SYSTEM_STATE_MANUAL: - } else if (current_status->arming_state == ARMING_STATE_ARMED) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; + ret = OK; } - 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: + case SYSTEM_STATE_SEATBELT: - if (current_status->arming_state == ARMING_STATE_ARMED) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_AUTO) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); - valid_transition = true; + ret = OK; } - 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: + case SYSTEM_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_ERROR) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT) { - valid_transition = true; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + ret = OK; } break; - case ARMING_STATE_IN_AIR_RESTORE: + case SYSTEM_STATE_REBOOT: - 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; - } - } + /* from INIT you can go straight to REBOOT */ + if (current_system_state == SYSTEM_STATE_INIT) { - 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; + ret = OK; } break; - case HIL_STATE_ON: + case SYSTEM_STATE_IN_AIR_RESTORE: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + /* from INIT you can go straight to an IN AIR RESTORE */ + if (current_system_state == SYSTEM_STATE_INIT) { - current_status->flag_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; + ret = OK; } 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 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; +//} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf..57b3db8f1 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,9 +47,9 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +int 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); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 27a471f13..20cb25cc0 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,27 +60,48 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_STANDBY=0, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_DESCENT, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TAKEOFF -} navigation_state_t; + SYSTEM_STATE_INIT=0, + SYSTEM_STATE_MANUAL, + SYSTEM_STATE_SEATBELT, + SYSTEM_STATE_AUTO, + SYSTEM_STATE_REBOOT, + SYSTEM_STATE_IN_AIR_RESTORE +} system_state_t; typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ABORT, - ARMING_STATE_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE -} arming_state_t; + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + SEATBELT_STANDBY, + SEATBELT_READY, + SEATBELT, + SEATBELT_ASCENT, + SEATBELT_DESCENT, +} seatbelt_state_t; + +typedef enum { + AUTO_STANDBY, + AUTO_READY, + AUTO_LOITER, + AUTO_MISSION, + AUTO_RTL, + AUTO_TAKEOFF, + AUTO_LAND, +} auto_state_t; + +//typedef enum { +// ARMING_STATE_INIT = 0, +// ARMING_STATE_STANDBY, +// ARMING_STATE_ARMED_GROUND, +// ARMING_STATE_ARMED_AIRBORNE, +// ARMING_STATE_ERROR_GROUND, +// ARMING_STATE_ERROR_AIRBORNE, +// ARMING_STATE_REBOOT, +// ARMING_STATE_IN_AIR_RESTORE +//} arming_state_t; typedef enum { HIL_STATE_OFF = 0, @@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, + MODE_SWITCH_SEATBELT, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -114,26 +135,6 @@ typedef enum { 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 */ -// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ -// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -//}; -// -//enum VEHICLE_MANUAL_CONTROL_MODE { -// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ -// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ -// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -//}; -// -//enum VEHICLE_MANUAL_SAS_MODE { -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ -// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ -// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -//}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -180,8 +181,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current navigation state */ - arming_state_t arming_state; /**< current arming state */ + system_state_t system_state; /**< current system state */ +// arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -194,6 +195,7 @@ struct vehicle_status_s 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_in_air_restore; /**< true if we can restore in mid air */ @@ -212,9 +214,6 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_land_requested; /**< true if land requested */ - bool flag_mission_activated; /**< true if in mission mode */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; |