From d5eae460c0e4eab4e1bf56a16d8c16dc8cc7ee6f Mon Sep 17 00:00:00 2001 From: sander Date: Fri, 5 Feb 2016 13:06:51 +0100 Subject: [PATCH] VTOL transition failsafe RTL --- msg/vehicle_status.msg | 2 ++ msg/vtol_vehicle_status.msg | 9 ++++---- src/lib/ecl | 2 +- src/modules/commander/commander.cpp | 21 ++++++++++++++----- .../commander/state_machine_helper.cpp | 10 +++++++-- .../vtol_att_control_main.cpp | 5 +++-- src/modules/vtol_att_control/vtol_type.cpp | 2 +- src/modules/vtol_att_control/vtol_type.h | 2 +- 8 files changed, 37 insertions(+), 16 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 4b561be28c..e7ddca892a 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -150,6 +150,8 @@ bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events bool engine_failure # Set to true if an engine failure is detected bool engine_failure_cmd # Set to true if an engine failure mode is commanded +bool vtol_transition_failure # Set to true if vtol transition failed +bool vtol_transition_failure_cmd # Set to true if vtol transition failure mode is commanded bool gps_failure # Set to true if a gps failure is detected bool gps_failure_cmd # Set to true if a gps failure mode is commanded diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index a1be5d5867..6fce6b330e 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -1,5 +1,6 @@ -uint64 timestamp # Microseconds since system boot -bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode +uint64 timestamp # Microseconds since system boot +bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode bool vtol_in_trans_mode -bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode -float32 airspeed_tot # Estimated airspeed over control surfaces +bool vtol_transition_failsafe # vtol in transition failsafe mode +bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode +float32 airspeed_tot # Estimated airspeed over control surfaces diff --git a/src/lib/ecl b/src/lib/ecl index 0a9f7e58db..b21a49b3f9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 0a9f7e58dbb5708fc894a865376ede6cbb675fd4 +Subproject commit b21a49b3f9b7267663fd79a92f50c2db78eae762 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 950458f218..140d6b4c88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -35,11 +35,12 @@ * @file commander.cpp * Main fail-safe handling. * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * @author Sander Smeets */ #include @@ -796,6 +797,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s status_local->data_link_lost_cmd = false; status_local->gps_failure_cmd = false; status_local->rc_signal_lost_cmd = false; + status_local->vtol_transition_failure_cmd = false; if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ @@ -820,8 +822,15 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* trigger rc loss mode */ status_local->rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); + + } else if ((int)cmd->param2 == 5) { + /* trigger vtol transition failure mode */ + status_local->vtol_transition_failure_cmd = true; + warnx("vtol transition failure mode commanded"); } + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -1718,6 +1727,8 @@ int commander_thread_main(int argc, char *argv[]) if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; + status.vtol_transition_failure = vtol_status.vtol_transition_failsafe; + status.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe; } status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index db151fbe5c..966dc3b189 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -35,8 +35,9 @@ * @file state_machine_helper.cpp * State machine helper functions implementations * - * @author Thomas Gubler - * @author Julian Oes + * @author Thomas Gubler + * @author Julian Oes + * @author Sander Smeets */ #include @@ -636,6 +637,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe * - if commanded to do so * - if we have an engine failure + * - if we have vtol transition failure * - depending on datalink, RC and if the mission is finished */ /* first look at the commands */ @@ -647,12 +649,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->vtol_transition_failure_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->vtol_transition_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 780b8e8cf8..3a10472ca0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -454,6 +454,7 @@ VtolAttitudeControl::abort_front_transition() if(!_abort_front_transition) { mavlink_log_critical(_mavlink_fd, "Front transition timeout occured, aborting"); _abort_front_transition = true; + _vtol_vehicle_status.vtol_transition_failsafe = true; } } @@ -739,8 +740,8 @@ void VtolAttitudeControl::task_main() /* Only publish if the proper mode(s) are enabled */ if (_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) { + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 2f361eb8f4..db4c472e0c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** -* @file airframe.cpp +* @file vtol_type.cpp * * @author Roman Bapst * diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 2f9211885b..b8cfa81fb6 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** -* @file airframe.h +* @file vtol_type.h * * @author Roman Bapst *