diff options
Diffstat (limited to 'apps/commander/state_machine_helper.c')
-rw-r--r-- | apps/commander/state_machine_helper.c | 564 |
1 files changed, 564 insertions, 0 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c new file mode 100644 index 000000000..2171ccedf --- /dev/null +++ b/apps/commander/state_machine_helper.c @@ -0,0 +1,564 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * + * 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 functions implementations */ + +#include <stdio.h> +#include "state_machine_helper.h" + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <systemlib/systemlib.h> +#include <arch/board/up_hrt.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", + +}; + + +void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state) +{ + int invalid_state = false; + + switch (new_state) { + case SYSTEM_STATE_MISSION_ABORT: { + /* Indoor or outdoor */ + uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + + if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + + } else { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + } + + return; + } + break; + + case SYSTEM_STATE_EMCY_LANDING: + /* Tell the controller to land */ + //TODO: add emcy landing code here + + fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + /* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */ + //TODO: add emcy cutoff code here + + fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_GROUND_ERROR: + fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_PREFLIGHT: + //global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_REBOOT: + usleep(500000); + reboot(); + break; + + case SYSTEM_STATE_STANDBY: + //global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_GROUND_READY: + //global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO); + + //if in manual mode switch to manual state +// if (current_status->remote_manual) { +// printf("[commander] manual mode\n"); +// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); +// return; +// } + + break; + + case SYSTEM_STATE_AUTO: + //global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_STABILIZED: + //global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_MANUAL: + //global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false) { + //publish the new state + current_status->state_machine = new_state; + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[commander] new state: %s\n", system_state_txt[new_state]); + } +} + + +/* + * Private functions, update the state machine + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status) +{ + fprintf(stderr, "[commander] 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, (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_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + } else { + fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + } +} + +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //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); + + } else { + //global_data_send_mavlink_statustext_message_out("[commander] 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*/ + + +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status) +{ + /* 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); + } +} + +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + printf("[commander] arming\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + } /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) { + + printf("[commander] landing\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + }*/ +} + +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + printf("[commander] going standby\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + + } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[commander] MISSION ABORT!\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[commander] manual mode\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + } +} + +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[commander] stabilized mode\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + } +} + +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { + printf("[commander] auto mode\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO); + } +} + + +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode) +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + printf("in update state request\n"); + uint8_t ret = 1; + + /* Switch on HIL if in standby */ + if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { + /* Enable HIL on request */ + current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* NEVER actually switch off HIL without reboot */ + if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { + fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + ret = ERROR; + } + + //TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c) + switch (mode) { + case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED + if (current_system_state == SYSTEM_STATE_STANDBY) { + /* Set armed flag */ + current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; + /* Set manual input enabled flag */ + current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + ret = OK; + } + + break; + + case MAV_MODE_MANUAL_DISARMED: + if (current_system_state == SYSTEM_STATE_GROUND_READY) { + /* Clear armed flag, leave manual input enabled */ + current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + } + + break; + + default: + break; + } + + #warning STATE MACHINE IS INCOMPLETE HERE + +// if(ret != 0) //Debug +// { +// strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected"); +// global_data_send_mavlink_message_out(&mavlink_statustext_msg_content); +// } + + return ret; +} + +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, 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) { + printf("system will reboot\n"); + //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + do_state_update(status_pub, current_status, (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, (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, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + printf("state: manual\n"); + ret = 0; + } + + break; + + default: + break; + } + + return ret; +} + |