diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 6c92ae83fd72..594849a2c8c9 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -78,6 +78,7 @@ bool is_vtol_tailsitter # True if the system performs a 90° pit bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode bool in_transition_mode # True if VTOL is doing a transition bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW +bool vtol_fw_actuation_failure # True if an issue with fixed-wing actuation was detected bool rc_signal_lost # true if RC reception lost diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index ee76c13a4e94..438b07ceee56 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -38,7 +38,6 @@ bool offboard_control_signal_lost bool rc_signal_found_once bool rc_calibration_in_progress bool rc_calibration_valid # set if RC calibration is valid -bool vtol_transition_failure # Set to true if vtol transition failed bool usb_connected # status of the USB power supply bool sd_card_detected_once # set to true if the SD card was detected diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c62266be03e4..6addc122bff1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2259,11 +2259,6 @@ Commander::run() _status_changed = true; } - if (_status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) { - _status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe; - _status_changed = true; - } - const bool should_soft_stop = (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); if (_armed.soft_stop != should_soft_stop) { @@ -2768,7 +2763,32 @@ Commander::run() } } - if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) { + const bool flight_state_enable_quadchute = _status.is_vtol + && (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _status.in_transition_mode); + + if (flight_state_enable_quadchute && !_quadchute_triggered) { + if (fd_status_flags.qc_roll || fd_status_flags.qc_pitch || fd_status_flags.qc_min_alt || fd_status_flags.qc_alt_err + || fd_status_flags.qc_trans_tmt) { + _quadchute_triggered = true; + mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: Transition to hover\t"); + /* EVENT + * @description + * Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger. + * + * + * Quadchute can be disabled with the parameter QC_enabled. + * + */ + events::send(events::ID("commander_fd_quadchute"), {events::Log::Emergency, events::LogInternal::Warning}, + "Critical failure detected: transition to hover"); + // send_quadchute_command(); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, + 1.0f); + _status.vtol_fw_actuation_failure = true; + _status_changed = true; + } + + } else if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) { const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); if (is_right_after_takeoff && !_lockdown_triggered) { @@ -4389,6 +4409,18 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } +// void Commander::send_quadchute_command() +// { +// uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}; +// vehicle_status_s vehicle_status{}; +// vehicle_status_sub.copy(&vehicle_status); +// send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, +// (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? +// vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : +// vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f); + +// } + void Commander::checkWindAndWarn() { wind_s wind_estimate; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 62a3b43b7caa..533ade69b4e8 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -322,7 +322,7 @@ class Commander : public ModuleBase, public ModuleParams bool _flight_termination_triggered{false}; bool _lockdown_triggered{false}; bool _imbalanced_propeller_check_triggered{false}; - + bool _quadchute_triggered{false}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index e57d4a940b39..2c769fcbfb35 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 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 @@ -52,7 +52,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic failure_detector_status_u status_prev = _status; if (vehicle_control_mode.flag_control_attitude_enabled) { - updateAttitudeStatus(); + updateAttitudeStatus(vehicle_status); if (_param_fd_ext_ats_en.get()) { updateExternalAtsStatus(); @@ -73,10 +73,81 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic updateImbalancedPropStatus(); } + + if (vehicle_status.is_vtol) { + updateMinHeightStatus(vehicle_status); + updateAdaptiveQC(vehicle_status, vehicle_control_mode); + updateTransitionTimeout(); + } + return _status.value != status_prev.value; } -void FailureDetector::updateAttitudeStatus() +void FailureDetector::updateMinHeightStatus(const vehicle_status_s &vehicle_status) +{ + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.update(&vehicle_local_position)) { + if (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && _param_vt_fw_min_alt.get() > 0 + && -(vehicle_local_position.z) < _param_vt_fw_min_alt.get()) { + _status.flags.qc_min_alt = true; + + } else { + _status.flags.qc_min_alt = false; + } + } +} + +void FailureDetector::updateAdaptiveQC(const vehicle_status_s &vehicle_status, + const vehicle_control_mode_s &vehicle_control_mode) +{ + // adaptive quadchute, only enabled when TECS is running (so pure FW mode) + if (_param_vt_fw_alt_err.get() > 0 && vehicle_control_mode.flag_control_climb_rate_enabled + && vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + tecs_status_s tecs_status; + + if (_tecs_status_sub.update(&tecs_status)) { + // 1 second rolling average + _ra_hrate = (49 * _ra_hrate + tecs_status.height_rate) / 50; + _ra_hrate_sp = (49 * _ra_hrate_sp + tecs_status.height_rate_setpoint) / 50; + + // are we dropping while requesting significant ascend? + if (((tecs_status.altitude_sp - tecs_status.altitude_filtered) > _param_vt_fw_alt_err.get()) && + (_ra_hrate < -1.0f) && (_ra_hrate_sp > 1.0f)) { + + _status.flags.qc_alt_err = true; + } + } + + } else { + // reset the filtered height rate and heigh rate setpoint if TECS is not running + _ra_hrate = 0.0f; + _ra_hrate_sp = 0.0f; + } +} + +void FailureDetector::updateTransitionTimeout() +{ + vtol_vehicle_status_s vtol_vehicle_status; + _vtol_vehicle_status_sub.update(&vtol_vehicle_status); + + const bool transitioning_to_FW = vtol_vehicle_status.in_transition_to_fw; + + if (!_was_in_transition_FW_prev && transitioning_to_FW) { + _transition_start_timestamp = hrt_absolute_time(); + _was_in_transition_FW_prev = true; + } + + if (transitioning_to_FW && _param_vt_trans_timeout.get() > FLT_EPSILON) { + if (hrt_elapsed_time(&_transition_start_timestamp) > _param_vt_trans_timeout.get() * 1_s) { + // transition timeout occured, abort transition + _status.flags.qc_trans_tmt = true; + } + } +} + +void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status) { vehicle_attitude_s attitude; @@ -105,6 +176,31 @@ void FailureDetector::updateAttitudeStatus() // Update status _status.flags.roll = _roll_failure_hysteresis.get_state(); _status.flags.pitch = _pitch_failure_hysteresis.get_state(); + + const bool enable_quadchte = vehicle_status.is_vtol + && vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + if (enable_quadchte) { + + // fixed-wing maximum roll angle + if (_param_vt_fw_qc_r.get() > 0) { + if (fabsf(roll) > fabsf(math::radians(_param_vt_fw_qc_r.get()))) { + _status.flags.qc_roll = true; + } + } + + // fixed-wing maximum pitch angle + if (_param_vt_fw_qc_p.get() > 0) { + if (fabsf(pitch) > fabsf(math::radians(_param_vt_fw_qc_p.get()))) { + _status.flags.qc_pitch = true; + } + } + + } else { + // reset if not in transition or FW + _status.flags.qc_roll = true; + _status.flags.qc_pitch = true; + } } } diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index bf9d164d305a..b9d406a65da4 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 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 @@ -52,11 +52,15 @@ // subscriptions #include #include +#include #include #include #include #include +#include +#include #include +#include #include #include @@ -70,6 +74,11 @@ union failure_detector_status_u { uint16_t high_wind : 1; uint16_t battery : 1; uint16_t imbalanced_prop : 1; + uint16_t qc_roll : 1; + uint16_t qc_pitch : 1; + uint16_t qc_min_alt : 1; + uint16_t qc_alt_err : 1; + uint16_t qc_trans_tmt : 1; } flags; uint16_t value {0}; }; @@ -87,10 +96,13 @@ class FailureDetector : public ModuleParams float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } private: - void updateAttitudeStatus(); + void updateAttitudeStatus(const vehicle_status_s &vehicle_status); void updateExternalAtsStatus(); void updateEscsStatus(const vehicle_status_s &vehicle_status); void updateImbalancedPropStatus(); + void updateMinHeightStatus(const vehicle_status_s &vehicle_status); + void updateAdaptiveQC(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); + void updateTransitionTimeout(); failure_detector_status_u _status{}; @@ -110,6 +122,17 @@ class FailureDetector : public ModuleParams uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint}); + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + + float _ra_hrate{0.f}; + float _ra_hrate_sp{0.f}; + + hrt_abstime _transition_start_timestamp{0}; + bool _was_in_transition_FW_prev{false}; + DEFINE_PARAMETERS( (ParamInt) _param_fd_fail_p, (ParamInt) _param_fd_fail_r, @@ -118,6 +141,12 @@ class FailureDetector : public ModuleParams (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, (ParamInt) _param_escs_en, - (ParamInt) _param_fd_imb_prop_thr + (ParamInt) _param_fd_imb_prop_thr, + + (ParamInt) _param_vt_fw_min_alt, + (ParamInt) _param_vt_fw_alt_err, + (ParamInt) _param_vt_fw_qc_p, + (ParamInt) _param_vt_fw_qc_r, + (ParamFloat) _param_vt_trans_timeout ) }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index 01444324f9ba..1b0106dad777 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 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 @@ -156,3 +156,63 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1); * @group Failure Detector */ PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30); + +/** + * QuadChute minimum height threshold + * + * If the current height (relative to takeoff point) drops below this value in fixed-wing mode, a "QuadChute" is triggered, + * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT. + * Only enabled for VTOL vehicles. + * Set to 0 to disable the check. + * + * @min 0 + * @max 500 + * @unit m + * @increment 1 + * @group Failure Detector + */ +PARAM_DEFINE_INT32(VT_FW_MIN_ALT, 0); + +/** + * QuadChute altitude error thershold + * + * If the current altitude is more than this amount below the setpoint, a "QuadChute" is triggered, + * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT. + * Only enabled for VTOL vehicles. + * Set to 0 to disable the check. + * + * @min 0 + * @max 200 + * @unit m + * @increment 1 + * @group Failure Detector + */ +PARAM_DEFINE_INT32(VT_FW_ALT_ERR, 0); + +/** + * QuadChute pitch threshold + * + * If the current pitch angle estimate is above this value, a "QuadChute" is triggered, + * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT. + * Set to 0 to disable the check. + * + * @min 0 + * @max 180 + * @unit deg + * @group Failure Detector + */ +PARAM_DEFINE_INT32(VT_FW_QC_P, 0); + +/** + * QuadChute roll threshold + * + * If the current roll angle estimate is above this value, a "QuadChute" is triggered, + * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT. + * Set to 0 to disable the check. + * + * @min 0 + * @max 180 + * @unit deg + * @group Failure Detector + */ +PARAM_DEFINE_INT32(VT_FW_QC_R, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6ed5d5376166..71282caee98c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -592,7 +592,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (status.engine_failure) { status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags.vtol_transition_failure) { + } else if (status.vtol_fw_actuation_failure) { set_quadchute_nav_state(status, armed, status_flags, quadchute_act); @@ -646,7 +646,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state - } else if (status_flags.vtol_transition_failure) { + } else if (status.vtol_fw_actuation_failure) { set_quadchute_nav_state(status, armed, status_flags, quadchute_act); @@ -762,7 +762,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { // nothing to do - everything done in check_invalid_pos_nav_state - } else if (status_flags.vtol_transition_failure) { + } else if (status.vtol_fw_actuation_failure) { set_quadchute_nav_state(status, armed, status_flags, quadchute_act); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5b1c36b8ee0c..13e2b63c0624 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -107,17 +107,12 @@ void Standard::update_vtol_state() float mc_weight = _mc_roll_weight; float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; - if (_vtol_vehicle_status->vtol_transition_failsafe) { + if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) { // Failsafe event, engage mc motors immediately _vtol_schedule.flight_mode = vtol_mode::MC_MODE; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - //reset failsafe when FW is no longer requested - if (!_attc->is_fixed_wing_requested()) { - _vtol_vehicle_status->vtol_transition_failsafe = false; - } - } else if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off @@ -280,14 +275,6 @@ void Standard::update_transition_state() const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); - // check front transition timeout - if (_params->front_trans_timeout > FLT_EPSILON) { - if (time_since_trans_start > _params->front_trans_timeout) { - // transition timeout occured, abort transition - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); - } - } - } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { if (_v_control_mode->flag_control_climb_rate_enabled) { diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d583ab447375..4460cf3cd71b 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -82,15 +82,10 @@ void Tailsitter::update_vtol_state() float pitch = Eulerf(Quatf(_v_att->q)).theta(); - if (_vtol_vehicle_status->vtol_transition_failsafe) { + if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) { // Failsafe event, switch to MC mode immediately _vtol_schedule.flight_mode = vtol_mode::MC_MODE; - //reset failsafe when FW is no longer requested - if (!_attc->is_fixed_wing_requested()) { - _vtol_vehicle_status->vtol_transition_failsafe = false; - } - } else if (!_attc->is_fixed_wing_requested()) { switch (_vtol_schedule.flight_mode) { // user switchig to MC mode @@ -132,9 +127,6 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_FRONT_P1: { - const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; - - const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && !_params->airspeed_disabled; @@ -155,14 +147,6 @@ void Tailsitter::update_vtol_state() _vtol_schedule.flight_mode = vtol_mode::FW_MODE; } - // check front transition timeout - if (_params->front_trans_timeout > FLT_EPSILON) { - if (time_since_trans_start > _params->front_trans_timeout) { - // transition timeout occured, abort transition - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); - } - } - break; } @@ -252,14 +236,6 @@ void Tailsitter::update_transition_state() time_since_trans_start * trans_pitch_rate)) * _q_trans_start; } - // check front transition timeout - if (_params->front_trans_timeout > FLT_EPSILON) { - if (time_since_trans_start > _params->front_trans_timeout) { - // transition timeout occured, abort transition - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); - } - } - } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ae4b843fef47..4a05e079cd9c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -105,15 +105,10 @@ void Tiltrotor::update_vtol_state() * forward completely. For the backtransition the motors simply rotate back. */ - if (_vtol_vehicle_status->vtol_transition_failsafe) { + if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) { // Failsafe event, switch to MC mode immediately _vtol_schedule.flight_mode = vtol_mode::MC_MODE; - //reset failsafe when FW is no longer requested - if (!_attc->is_fixed_wing_requested()) { - _vtol_vehicle_status->vtol_transition_failsafe = false; - } - } else if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode @@ -199,14 +194,6 @@ void Tiltrotor::update_vtol_state() _vtol_schedule.transition_start = hrt_absolute_time(); } - // check front transition timeout - if (_params->front_trans_timeout > FLT_EPSILON) { - if (time_since_trans_start > _params->front_trans_timeout) { - // transition timeout occured, abort transition - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); - } - } - break; } 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 318eb5e1d2bd..45b2e98a9c85 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -78,10 +78,6 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); - _params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR"); - _params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P"); - _params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R"); _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); @@ -180,8 +176,6 @@ void VtolAttitudeControl::vehicle_cmd_poll() while (_vehicle_cmd_sub.update(&vehicle_command)) { if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; @@ -189,10 +183,10 @@ void VtolAttitudeControl::vehicle_cmd_poll() // deny transition from MC to FW in Takeoff, Land, RTL and Orbit if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW && - (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) { + (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) { result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; @@ -216,58 +210,6 @@ void VtolAttitudeControl::vehicle_cmd_poll() } } -void -VtolAttitudeControl::quadchute(QuadchuteReason reason) -{ - if (!_vtol_vehicle_status.vtol_transition_failsafe) { - switch (reason) { - case QuadchuteReason::TransitionTimeout: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: transition timeout\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_tout"), events::Log::Critical, - "Quadchute triggered, due to transition timeout"); - break; - - case QuadchuteReason::ExternalCommand: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: external command\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_ext_cmd"), events::Log::Critical, - "Quadchute triggered, due to external command"); - break; - - case QuadchuteReason::MinimumAltBreached: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: minimum altitude breached\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_min_alt"), events::Log::Critical, - "Quadchute triggered, due to minimum altitude breach"); - break; - - case QuadchuteReason::LossOfAlt: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical, - "Quadchute triggered, due to loss of altitude"); - break; - - case QuadchuteReason::LargeAltError: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: large altitude error\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_alt_err"), events::Log::Critical, - "Quadchute triggered, due to large altitude error"); - break; - - case QuadchuteReason::MaximumPitchExceeded: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum pitch exceeded\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_max_pitch"), events::Log::Critical, - "Quadchute triggered, due to maximum pitch angle exceeded"); - break; - - case QuadchuteReason::MaximumRollExceeded: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum roll exceeded\t"); - events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical, - "Quadchute triggered, due to maximum roll angle exceeded"); - break; - } - - _vtol_vehicle_status.vtol_transition_failsafe = true; - } -} - int VtolAttitudeControl::parameters_update() { @@ -296,22 +238,6 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.elevons_mc_lock, &l); _params.elevons_mc_lock = (l == 1); - /* minimum relative altitude for FW mode (QuadChute) */ - param_get(_params_handles.fw_min_alt, &v); - _params.fw_min_alt = v; - - /* maximum negative altitude error for FW mode (Adaptive QuadChute) */ - param_get(_params_handles.fw_alt_err, &v); - _params.fw_alt_err = v; - - /* maximum pitch angle (QuadChute) */ - param_get(_params_handles.fw_qc_max_pitch, &l); - _params.fw_qc_max_pitch = l; - - /* maximum roll angle (QuadChute) */ - param_get(_params_handles.fw_qc_max_roll, &l); - _params.fw_qc_max_roll = l; - param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop); param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min); @@ -457,6 +383,7 @@ VtolAttitudeControl::Run() _airspeed_validated_sub.update(&_airspeed_validated); _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); + _vehicle_status_sub.update(&_vehicle_status); action_request_poll(); vehicle_cmd_poll(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 39e7a9322e75..0176805f5d73 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -96,16 +96,6 @@ class VtolAttitudeControl : public ModuleBase, public px4:: { public: - enum class QuadchuteReason { - TransitionTimeout = 0, - ExternalCommand, - MinimumAltBreached, - LossOfAlt, - LargeAltError, - MaximumPitchExceeded, - MaximumRollExceeded, - }; - VtolAttitudeControl(); ~VtolAttitudeControl() override; @@ -121,10 +111,8 @@ class VtolAttitudeControl : public ModuleBase, public px4:: bool init(); bool is_fixed_wing_requested() { return _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; }; - void quadchute(QuadchuteReason reason); int get_transition_command() {return _transition_command;} bool get_immediate_transition() {return _immediate_transition;} - void reset_immediate_transition() {_immediate_transition = false;} float getAirDensity() const { return _air_density; } @@ -144,6 +132,7 @@ class VtolAttitudeControl : public ModuleBase, public px4:: struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + struct vehicle_status_s *get_vehicle_status() {return &_vehicle_status;} struct vehicle_torque_setpoint_s *get_torque_setpoint_0() {return &_torque_setpoint_0;} struct vehicle_torque_setpoint_s *get_torque_setpoint_1() {return &_torque_setpoint_1;} @@ -209,6 +198,7 @@ class VtolAttitudeControl : public ModuleBase, public px4:: vehicle_local_position_s _local_pos{}; vehicle_local_position_setpoint_s _local_pos_sp{}; vtol_vehicle_status_s _vtol_vehicle_status{}; + vehicle_status_s _vehicle_status{}; float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3] @@ -220,10 +210,6 @@ class VtolAttitudeControl : public ModuleBase, public px4:: param_t vtol_fw_permanent_stab; param_t vtol_type; param_t elevons_mc_lock; - param_t fw_min_alt; - param_t fw_alt_err; - param_t fw_qc_max_pitch; - param_t fw_qc_max_roll; param_t front_trans_time_openloop; param_t front_trans_time_min; param_t front_trans_duration; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 72c927c09e98..5f9339823327 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -218,50 +218,6 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f); */ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f); -/** - * QuadChute Altitude - * - * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude - * the vehicle will transition back to MC mode and enter failsafe RTL - * @min 0.0 - * @max 200.0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); - -/** - * Adaptive QuadChute - * - * Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint - * the vehicle will transition back to MC mode and enter failsafe RTL. - * @min 0.0 - * @max 200.0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f); - -/** - * QuadChute Max Pitch - * - * Maximum pitch angle before QuadChute engages - * Above this the vehicle will transition back to MC mode and enter failsafe RTL - * @min 0 - * @max 180 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_QC_P, 0); - -/** - * QuadChute Max Roll - * - * Maximum roll angle before QuadChute engages - * Above this the vehicle will transition back to MC mode and enter failsafe RTL - * @min 0 - * @max 180 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_QC_R, 0); - /** * Airspeed less front transition time (open loop) * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index bbf9bf1dba82..d8ebac058b01 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -203,8 +203,6 @@ void VtolType::update_fw_state() blendThrottleAfterFrontTransition(progress); } } - - check_quadchute_condition(); } void VtolType::update_transition_state() @@ -214,10 +212,6 @@ void VtolType::update_transition_state() _transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f); _last_loop_ts = t_now; _throttle_blend_start_ts = t_now; - - - - check_quadchute_condition(); } float VtolType::update_and_get_backtransition_pitch_sp() @@ -252,80 +246,6 @@ bool VtolType::can_transition_on_ground() return !_v_control_mode->flag_armed || _land_detected->landed; } -void VtolType::check_quadchute_condition() -{ - if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition() - && !_quadchute_command_treated) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand); - _quadchute_command_treated = true; - _attc->reset_immediate_transition(); - - } else { - _quadchute_command_treated = false; - } - - if (!_tecs_running) { - // reset the filtered height rate and heigh rate setpoint if TECS is not running - _ra_hrate = 0.0f; - _ra_hrate_sp = 0.0f; - } - - if (_v_control_mode->flag_armed && !_land_detected->landed) { - Eulerf euler = Quatf(_v_att->q); - - // fixed-wing minimum altitude - if (_params->fw_min_alt > FLT_EPSILON) { - - if (-(_local_pos->z) < _params->fw_min_alt) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached); - } - } - - // adaptive quadchute - if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) { - - // We use tecs for tracking in FW and local_pos_sp during transitions - if (_tecs_running) { - // 1 second rolling average - _ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50; - _ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50; - - // are we dropping while requesting significant ascend? - if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) && - (_ra_hrate < -1.0f) && - (_ra_hrate_sp > 1.0f)) { - - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt); - } - - } else { - const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err); - const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f); - - if (height_error && height_rate_error) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError); - } - } - } - - // fixed-wing maximum pitch angle - if (_params->fw_qc_max_pitch > 0) { - - if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded); - } - } - - // fixed-wing maximum roll angle - if (_params->fw_qc_max_roll > 0) { - - if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) { - _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded); - } - } - } -} - bool VtolType::set_idle_mc() { if (_params->ctrl_alloc == 1) { @@ -580,7 +500,7 @@ float VtolType::pusher_assist() } // Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive) - if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) { + if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) { return 0.0f; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1476a53d7675..99aea6ab5345 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -53,10 +53,6 @@ struct Params { int32_t vtol_motor_id; int32_t vtol_type; bool elevons_mc_lock; // lock elevons in multicopter mode - float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) - float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute) - float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute) - float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute) float front_trans_time_openloop; float front_trans_time_min; float front_trans_duration; @@ -174,11 +170,6 @@ class VtolType */ virtual void waiting_on_tecs() {} - /** - * Checks for fixed-wing failsafe condition and issues abort request if needed. - */ - void check_quadchute_condition(); - /** * Returns true if we're allowed to do a mode transition on the ground. */ @@ -227,6 +218,7 @@ class VtolType struct airspeed_validated_s *_airspeed_validated; // airspeed struct tecs_status_s *_tecs_status; struct vehicle_land_detected_s *_land_detected; + struct vehicle_status_s *_vehicle_status; struct vehicle_torque_setpoint_s *_torque_setpoint_0; struct vehicle_torque_setpoint_s *_torque_setpoint_1; @@ -247,9 +239,6 @@ class VtolType float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) float _last_thr_in_fw_mode = 0.0f; - float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition - float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition - bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition hrt_abstime _trans_finished_ts = 0; @@ -265,9 +254,6 @@ class VtolType float _accel_to_pitch_integ = 0; - bool _quadchute_command_treated{false}; - - /** * @brief Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures * that they are spinning in mc mode.