/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file state_machine_helper.c * State machine helper functions implementations */ #include #include #include #include #include #include #include #include #include #include #include "state_machine_helper.h" /** * Transition from one sytem state to another */ void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { int ret = ERROR; navigation_state_t new_navigation_state; /* do the navigation state update depending on the arming state */ switch (current_status->arming_state) { /* evaluate the navigation state when disarmed */ case ARMING_STATE_STANDBY: /* Always accept manual mode */ if (current_status->mode_switch == MODE_SWITCH_MANUAL) { new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; /* Accept SEATBELT if there is a local position estimate */ } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; } else { /* or just fall back to manual */ mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; } /* Accept AUTO if there is a global position estimate */ } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; } else { mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); /* otherwise fallback to seatbelt or even manual */ if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; } else { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; } } } break; /* evaluate the navigation state when armed */ case ARMING_STATE_ARMED: /* Always accept manual mode */ if (current_status->mode_switch == MODE_SWITCH_MANUAL) { new_navigation_state = NAVIGATION_STATE_MANUAL; /* Accept SEATBELT if there is a local position estimate */ } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT && current_status->return_switch == RETURN_SWITCH_NONE) { if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT; } else { /* or just fall back to manual */ mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL; } /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT && current_status->return_switch == RETURN_SWITCH_RETURN) { if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; } else { /* descent not possible without baro information, fall back to manual */ mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL; } /* Accept LOITER if there is a global position estimate */ } else if (current_status->mode_switch == MODE_SWITCH_AUTO && current_status->return_switch == RETURN_SWITCH_NONE && current_status->mission_switch == MISSION_SWITCH_NONE) { if (current_status->flag_global_position_valid) { new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; } else { mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); /* otherwise fallback to SEATBELT or even manual */ if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT; } else { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL; } } /* Accept MISSION if there is a global position estimate and home position */ } else if (current_status->mode_switch == MODE_SWITCH_AUTO && current_status->return_switch == RETURN_SWITCH_NONE && current_status->mission_switch == MISSION_SWITCH_MISSION) { if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; } else { /* spit out what exactly is unavailable */ if (current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); } else if (current_status->flag_valid_home_position) { mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); } else { mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); } /* otherwise fallback to SEATBELT or even manual */ if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT; } else { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL; } } /* Go to RTL or land if global position estimate and home position is available */ } else if (current_status->mode_switch == MODE_SWITCH_AUTO && current_status->return_switch == RETURN_SWITCH_RETURN && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { /* after RTL go to LAND */ if (current_status->flag_system_returned_to_home) { new_navigation_state = NAVIGATION_STATE_AUTO_LAND; } else { new_navigation_state = NAVIGATION_STATE_AUTO_RTL; } } else { /* spit out what exactly is unavailable */ if (current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); } else if (current_status->flag_valid_home_position) { mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); } else { mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); } /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ if (current_status->flag_local_position_valid) { new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; } else { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); new_navigation_state = NAVIGATION_STATE_MANUAL; } } } break; case ARMING_STATE_ARMED_ERROR: // TODO work out fail-safe scenarios break; case ARMING_STATE_STANDBY_ERROR: // TODO work out fail-safe scenarios break; case ARMING_STATE_REBOOT: // XXX I don't think we should end up here break; case ARMING_STATE_IN_AIR_RESTORE: // XXX not sure what to do here break; default: break; } /* Update the system state in case it has changed */ if (current_status->navigation_state != new_navigation_state) { /* Check if the transition is valid */ if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); } } return; } /* * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ if (new_navigation_state != current_navigation_state) { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } break; case NAVIGATION_STATE_MANUAL_STANDBY: /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_navigation_state == NAVIGATION_STATE_INIT || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; case NAVIGATION_STATE_MANUAL: /* all transitions possible */ ret = OK; break; case NAVIGATION_STATE_SEATBELT_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_navigation_state == NAVIGATION_STATE_INIT || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_navigation_state == NAVIGATION_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { ret = OK; } break; case NAVIGATION_STATE_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT || current_navigation_state == NAVIGATION_STATE_MANUAL || current_navigation_state == NAVIGATION_STATE_AUTO_LAND || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_navigation_state == NAVIGATION_STATE_AUTO_READY || current_navigation_state == NAVIGATION_STATE_AUTO_RTL || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { ret = OK; } break; case NAVIGATION_STATE_SEATBELT_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ if (current_navigation_state == NAVIGATION_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_MANUAL || current_navigation_state == NAVIGATION_STATE_AUTO_LAND || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_navigation_state == NAVIGATION_STATE_AUTO_READY || current_navigation_state == NAVIGATION_STATE_AUTO_RTL || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { ret = OK; } break; case NAVIGATION_STATE_AUTO_STANDBY: /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_navigation_state == NAVIGATION_STATE_INIT || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } break; case NAVIGATION_STATE_AUTO_READY: /* transitions from AUTO_STANDBY or AUTO_LAND */ if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { ret = OK; } break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } break; case NAVIGATION_STATE_AUTO_LOITER: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_navigation_state == NAVIGATION_STATE_AUTO_RTL || current_navigation_state == NAVIGATION_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; case NAVIGATION_STATE_AUTO_MISSION: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_navigation_state == NAVIGATION_STATE_AUTO_RTL || current_navigation_state == NAVIGATION_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; case NAVIGATION_STATE_AUTO_RTL: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_navigation_state == NAVIGATION_STATE_SEATBELT || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; case NAVIGATION_STATE_AUTO_LAND: /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } break; default: break; } } 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) { /* publish the new state */ current_status->counter++; 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; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } //void publish_armed_status(const struct vehicle_status_s *current_status) //{ // struct actuator_armed_s armed; // armed.armed = current_status->flag_system_armed; // /* 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); // } // //} // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status // */ // /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR // * // * START SUBSYSTEM/EMERGENCY FUNCTIONS // * */ // void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) // { // current_status->onboard_control_sensors_present |= 1 << *subsystem_type; // current_status->counter++; // current_status->timestamp = hrt_absolute_time(); // orb_publish(ORB_ID(vehicle_status), status_pub, current_status); // } // void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) // { // current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); // current_status->counter++; // current_status->timestamp = hrt_absolute_time(); // orb_publish(ORB_ID(vehicle_status), status_pub, current_status); // /* if a subsystem was removed something went completely wrong */ // switch (*subsystem_type) { // case SUBSYSTEM_TYPE_GYRO: // //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_ACC: // //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_MAG: // //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_GPS: // { // uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; // if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { // //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); // state_machine_emergency(status_pub, current_status); // } // } // break; // default: // break; // } // } // void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) // { // current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; // current_status->counter++; // current_status->timestamp = hrt_absolute_time(); // orb_publish(ORB_ID(vehicle_status), status_pub, current_status); // } // void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) // { // current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); // current_status->counter++; // current_status->timestamp = hrt_absolute_time(); // orb_publish(ORB_ID(vehicle_status), status_pub, current_status); // /* if a subsystem was disabled something went completely wrong */ // switch (*subsystem_type) { // case SUBSYSTEM_TYPE_GYRO: // //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_ACC: // //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_MAG: // //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); // state_machine_emergency_always_critical(status_pub, current_status); // break; // case SUBSYSTEM_TYPE_GPS: // { // uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); // if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { // //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); // state_machine_emergency(status_pub, current_status); // } // } // break; // default: // break; // } // } // 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*/ // //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_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; //} #if 0 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_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; } #endif