diff options
Diffstat (limited to 'src/modules/commander/state_machine_helper.c')
| -rw-r--r-- | src/modules/commander/state_machine_helper.c | 1085 |
1 files changed, 543 insertions, 542 deletions
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..daed81553 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -40,182 +40,495 @@ #include <stdio.h> #include <unistd.h> +#include <stdint.h> +#include <stdbool.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; +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; - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + current_state->flag_fmu_armed = false; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + default: + break; } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + } - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; + return ret; +} - case SYSTEM_STATE_GROUND_ERROR: - /* set system flags according to state */ - /* prevent actuators from arming */ - current_status->flag_system_armed = false; +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; + int ret = ERROR; - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: + + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_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; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* 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; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_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->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + 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->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_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->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_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->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + default: + break; } - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* 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 { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); } + } - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: + - /* set system flags according to state */ + return ret; +} - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; +/** +* 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; - case SYSTEM_STATE_AUTO: + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - /* set system flags according to state */ + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; + } else { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; + switch (new_state) { - case SYSTEM_STATE_STABILIZED: + case HIL_STATE_OFF: - /* set system flags according to state */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; - case SYSTEM_STATE_MANUAL: + case HIL_STATE_ON: - /* set system flags according to state */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; - default: - invalid_state = true; - break; + default: + warnx("Unknown hil state"); + break; + } } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->hil_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } 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) { /* publish the new state */ @@ -223,76 +536,41 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } + +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - - /* XXX allow arming by external components on multicopters only if not yet armed by RC */ - /* XXX allow arming only if core sensors are ok */ - armed.ready_to_arm = true; - - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - } +// void publish_armed_status(const struct vehicle_status_s *current_status) +// { +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// +// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ +// /* XXX allow arming only if core sensors are ok */ +// armed.ready_to_arm = true; +// +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +// } // /* @@ -405,353 +683,76 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - 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, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} |