From 278e05e425f6aca75e2d6b43a17945b095176997 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 16:52:41 +0100 Subject: added simple flight termination state machine which enbales parachute on request --- src/modules/commander/commander.cpp | 19 +++++++- src/modules/commander/state_machine_helper.cpp | 51 ++++++++++++++++++++++ src/modules/commander/state_machine_helper.h | 4 ++ src/modules/fw_att_control/fw_att_control_main.cpp | 44 +++++++++++++++---- src/modules/uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 8 ++++ 6 files changed, 117 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfd4d2f73..40562a4e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + /* Flight termination */ + case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command + + if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* reject parachute depoyment not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } + break; + default: break; } @@ -1199,6 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); + bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); @@ -1725,7 +1741,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) continue; /* only handle low-priority commands here */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a50af7daf..6c21dfab0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; static bool navigation_state_changed = true; +static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -451,6 +452,18 @@ check_navigation_state_changed() } } +bool +check_flighttermination_state_changed() +{ + if (flighttermination_state_changed) { + flighttermination_state_changed = false; + return true; + + } else { + return false; + } +} + void set_navigation_state_changed() { @@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s } +/** +* Transition from one flightermination state to another +*/ +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; +} + + // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0bfdf36a8..e1ec9d4ad 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -70,8 +70,12 @@ bool check_main_state_changed(); transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); + bool check_navigation_state_changed(); +bool check_flighttermination_state_changed(); + void set_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ff3f13306..bb74534f0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -121,6 +121,7 @@ private: orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ @@ -128,7 +129,8 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -294,14 +296,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _airspeed_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - accel_report _accel = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _vcontrol_mode = {0}; - actuator_controls_s _actuators = {0}; - vehicle_global_position_s _global_pos = {0}; + _att = {0}; + _accel = {0}; + _att_sp = {0}; + _manual = {0}; + _airspeed = {0}; + _vcontrol_mode = {0}; + _actuators = {0}; + _actuators_airframe = {0}; + _global_pos = {0}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -625,6 +628,15 @@ FixedwingAttitudeControl::task_main() lock_integrator = true; } + /* Simple handling of failsafe: deploy parachute if failsafe is on */ + if (_vcontrol_mode.flag_control_flighttermination_enabled) { + _actuators_airframe.control[1] = 1.0f; +// warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { + _actuators_airframe.control[1] = -1.0f; +// warnx("_actuators_airframe.control[1] = -1.0f;"); + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -794,6 +806,7 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -804,6 +817,19 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } + if (_actuators_1_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", + (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], + (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], + (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + + } else { + /* advertise and publish */ + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + } + } perf_end(_loop_perf); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..e26fb97c8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -78,6 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7..1c3763924 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -95,6 +95,12 @@ typedef enum { HIL_STATE_ON } hil_state_t; + +typedef enum { + FLIGHTTERMINATION_STATE_OFF = 0, + FLIGHTTERMINATION_STATE_ON +} flighttermination_state_t; + typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_ASSISTED, @@ -229,6 +235,8 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3