From 01e427b17c161d8adaa38d6bdb91aecb434451f2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 16:14:32 -0700 Subject: Merge working changes into export-build branch. --- src/modules/commander/state_machine_helper.c | 752 +++++++++++++++++++++++++++ 1 file changed, 752 insertions(+) create mode 100644 src/modules/commander/state_machine_helper.c (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c new file mode 100644 index 000000000..bea388a10 --- /dev/null +++ b/src/modules/commander/state_machine_helper.c @@ -0,0 +1,752 @@ +/**************************************************************************** + * + * 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 "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 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); + + // } else { + // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + // } + } + 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; + + warnx("EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); + break; + + case SYSTEM_STATE_GROUND_ERROR: + + /* set system flags according to state */ + + /* prevent actuators from arming */ + current_status->flag_system_armed = false; + + warnx("GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); + break; + + 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"); + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + } + + 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"); + } + + 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 */ + + /* ground ready has motors / actuators armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); + break; + + case SYSTEM_STATE_AUTO: + + /* set system flags according to state */ + + /* auto is airborne and in auto mode, motors armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); + break; + + case SYSTEM_STATE_STABILIZED: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); + break; + + case SYSTEM_STATE_MANUAL: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false || old_state != new_state) { + current_status->state_machine = 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; + } + + 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); + 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; + /* 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*/ + + +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; +} + -- cgit v1.2.3 From 79f9b61aff0570d1ab98dc5a4c7e6a71eec5009b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 May 2013 20:05:20 +0200 Subject: Fixed led patterns to be up to the latest specs --- src/drivers/px4io/px4io.cpp | 32 +++++++++++++++++----------- src/modules/commander/state_machine_helper.c | 5 +++++ src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/safety.c | 26 +++++++++++----------- src/modules/uORB/topics/actuator_controls.h | 1 + 5 files changed, 41 insertions(+), 26 deletions(-) (limited to 'src/modules/commander/state_machine_helper.c') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e69fbebf7..3006cf885 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -416,7 +416,7 @@ PX4IO::init() * already armed, and has been configured for in-air restart */ if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); @@ -500,7 +500,7 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); @@ -701,11 +701,18 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_ARM_OK; + if (armed.armed && !armed.lockdown) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { - clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } + if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { @@ -1271,9 +1278,9 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", - _battery_amp_per_volt, - _battery_amp_bias, - _battery_mamphour_total); + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); @@ -1303,9 +1310,10 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s\n", + printf("arming 0x%04x%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -1347,12 +1355,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) switch (cmd) { case PWM_SERVO_ARM: /* set the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); break; case PWM_SERVO_DISARM: /* clear the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; case PWM_SERVO_SET_UPDATE_RATE: diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index bea388a10..ab728c7bb 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -249,6 +249,11 @@ 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); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d015fb629..e575bd841 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -145,7 +145,8 @@ #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ -#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index a2880d2ef..f6cd5fb45 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Safety button logic. + * @file safety.c + * Safety button logic. */ #include @@ -56,10 +57,10 @@ static unsigned counter = 0; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0x000f /**< always on */ -#define LED_PATTERN_FMU_ARMED 0x5555 /**< slow blinking */ -#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ -#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< long off then double blink */ +#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ +#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ +#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ static unsigned blink_counter = 0; @@ -108,7 +109,8 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -119,8 +121,6 @@ safety_check_button(void *arg) counter++; } - /* Disarm quickly */ - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -137,18 +137,18 @@ safety_check_button(void *arg) } /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ - uint16_t pattern = LED_PATTERN_SAFE; + uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { - if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; } else { pattern = LED_PATTERN_IO_ARMED; } - } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { - pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { + pattern = LED_PATTERN_FMU_OK_TO_ARM; } diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 0b50a29ac..b7c4196c0 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; -- cgit v1.2.3