VTOL: Quad-chute: rework loss of altitude condition

Previously the condition was based on hard coded height rate estimate and
setpoint values and an altitude error threshold. That showed to be leading
to false positives when the vehicle doesn't tightly follow the altitdue
ramp given by TECS to achieve a new altitude setpoint, and has become
completely infeasibly with the latest TECS rework that leads to non-ramped
altitude setpoint changes in the tecs_status message.
The new check no longer checks the altitude error but only the height rate
instead. It begins to integrate the height rate error once it detects an
uncommanded descend condition (height rate negative while setpoint is
positive). Integral threshold can be tuned by user (VT_QC_HR_ERROR_I).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -213,10 +213,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
 				     "Quad-chute triggered due to minimum altitude breach");
 			break;
 
-		case QuadchuteReason::LossOfAlt:
-			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t");
+		case QuadchuteReason::UncommandedDescent:
+			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: Uncommanded descent detected\t");
 			events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical,
-				     "Quad-chute triggered due to loss of altitude");
+				     "Quad-chute triggered due to uncommanded descent detection");
 			break;
 
 		case QuadchuteReason::TransitionAltitudeLoss:

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -212,10 +212,14 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
 PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
 
 /**
- * Adaptive QuadChute
+ * Quad-chute uncommanded descent threshold
  *
- * 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.
+ * Threshold for integrated height rate error to trigger a uncommanded-descent quad-chute.
+ * Only checked in altitude-controlled fixed-wing flight.
+ * Additional conditions that have to be met for uncommanded descent detection are a positive (climbing) height
+ * rate setpoint and a negative (sinking) current height rate estimate.
+ *
+ * Set to 0 do disable this threshold.
  *
  * @unit m
  * @min 0.0
@@ -224,7 +228,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
  * @decimal 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
+PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
 
 /**
  * Quad-chute transition altitude loss threshold

src/modules/vtol_att_control/vtol_type.cpp

@@ -257,17 +257,28 @@ bool VtolType::isMinAltBreached()
 	return false;
 }
 
-bool VtolType::largeAltitudeLoss()
+bool VtolType::isUncommandedDescent()
 {
-	// adaptive quadchute
-	if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) {
+	if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled
+	    && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
 
-		// 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)) {
-			return true;
+		// TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here
+
+		if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) {
+			// vehicle is currently in uncommended descend, start integrating error
+
+			const hrt_abstime now = hrt_absolute_time();
+			float dt = static_cast<float>(now - _last_loop_quadchute_timestamp) / 1e6f;
+			dt = math::constrain(dt, 0.0001f, 0.1f);
+			_last_loop_quadchute_timestamp = now;
+
+			_height_rate_error_integral += (_tecs_status->height_rate_setpoint - _tecs_status->height_rate) * dt;
+
+		} else {
+			_height_rate_error_integral = 0.f; // reset
 		}
+
+		return (_height_rate_error_integral > _param_vt_qc_hr_error_i.get());
 	}
 
 	return false;
@@ -340,8 +351,8 @@ QuadchuteReason VtolType::getQuadchuteReason()
 		return QuadchuteReason::MinimumAltBreached;
 	}
 
-	if (largeAltitudeLoss()) {
-		return QuadchuteReason::LossOfAlt;
+	if (isUncommandedDescent()) {
+		return QuadchuteReason::UncommandedDescent;
 	}
 
 	if (isFrontTransitionAltitudeLoss()) {
@@ -363,20 +374,6 @@ QuadchuteReason VtolType::getQuadchuteReason()
 	return QuadchuteReason::None;
 }
 
-void VtolType::filterTecsHeightRates()
-{
-	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;
-
-	} 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 VtolType::handleSpecialExternalCommandQuadchute()
 {
 	if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
@@ -392,8 +389,6 @@ void VtolType::handleSpecialExternalCommandQuadchute()
 
 void VtolType::check_quadchute_condition()
 {
-
-	filterTecsHeightRates();
 	handleSpecialExternalCommandQuadchute();
 
 	if (isQuadchuteEnabled()) {

src/modules/vtol_att_control/vtol_type.h

@@ -89,7 +89,7 @@ enum class QuadchuteReason {
 	TransitionTimeout,
 	ExternalCommand,
 	MinimumAltBreached,
-	LossOfAlt,
+	UncommandedDescent,
 	TransitionAltitudeLoss,
 	MaximumPitchExceeded,
 	MaximumRollExceeded,
@@ -165,12 +165,11 @@ class VtolType : public ModuleParams
 	bool isMinAltBreached();
 
 	/**
-	 *  @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly.
-	 * 		This only applies when TECS is running.
+	 * @brief Indicates if conditions are met for uncommanded-descent quad-chute.
 	 *
-	 * @return     true if error larger than threshold
+	 * @return true if integrated height rate error larger than threshold
 	 */
-	bool largeAltitudeLoss();
+	bool isUncommandedDescent();
 
 	/**
 	 * @brief Indicates if there is an altitude loss higher than specified threshold during a VTOL transition to FW
@@ -200,11 +199,6 @@ class VtolType : public ModuleParams
 	 */
 	bool isFrontTransitionTimeout();
 
-	/**
-	 *  @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint.
-	 */
-	void filterTecsHeightRates();
-
 	/**
 	 *  @brief Special handling of QuadchuteReason::ReasonExternal
 	 */
@@ -290,8 +284,8 @@ class VtolType : public ModuleParams
 	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
+	float _height_rate_error_integral{0.f};
+
 
 	hrt_abstime _trans_finished_ts = 0;
 	hrt_abstime _transition_start_timestamp{0};
@@ -302,6 +296,7 @@ class VtolType : public ModuleParams
 
 	hrt_abstime _last_loop_ts = 0;
 	float _transition_dt = 0;
+	hrt_abstime _last_loop_quadchute_timestamp = 0;
 
 	float _accel_to_pitch_integ = 0;
 
@@ -319,7 +314,7 @@ class VtolType : public ModuleParams
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
 					(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
 					(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
-					(ParamFloat<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
+					(ParamFloat<px4::params::VT_QC_HR_ERROR_I>) _param_vt_qc_hr_error_i,
 					(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
 					(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
 					(ParamFloat<px4::params::VT_QC_T_ALT_LOSS>) _param_vt_qc_t_alt_loss,