Refactor uncommanded descend Quad-Chute (#21598)

* refactored uncommanded descend quadchute
- use fixed altitude error threshold
- compute error relative to higest altitude the vehicle has achieved
since it has flown below the altitude reference (setpoint)

* disabled altitude loss quadchute by default

* altitude loss quadchute: added protection against invalid local z


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -197,12 +197,12 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
 /**
  * Quad-chute uncommanded descent threshold
  *
- * 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.
+ * Altitude error threshold for quad-chute triggering during fixed-wing flight.
+ * The check is only active if altitude is controlled and the vehicle is below the current altitude reference.
+ * The altitude error is relative to the highest altitude the vehicle has achieved since it has flown below the current
+ * altitude reference.
  *
- * Set to 0 do disable this threshold.
+ * Set to 0 do disable.
  *
  * @unit m
  * @min 0.0
@@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
  * @decimal 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
+PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f);
 
 /**
  * Quad-chute transition altitude loss threshold

src/modules/vtol_att_control/vtol_type.cpp

@@ -272,26 +272,21 @@ bool VtolType::isMinAltBreached()
 
 bool VtolType::isUncommandedDescent()
 {
-	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) {
-
-		// TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here
+	const float current_altitude = -_local_pos->z + _local_pos->ref_alt;
 
-		if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) {
-			// vehicle is currently in uncommended descend, start integrating error
+	if (_param_vt_qc_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _local_pos->z_global
+	    && _v_control_mode->flag_control_altitude_enabled
+	    && PX4_ISFINITE(_tecs_status->altitude_reference)
+	    && (current_altitude < _tecs_status->altitude_reference)
+	    && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
 
-			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;
+		_quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude),
+					       _tecs_status->altitude_reference);
 
-			_height_rate_error_integral += (_tecs_status->height_rate_setpoint - _tecs_status->height_rate) * dt;
+		return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get();
 
-		} else {
-			_height_rate_error_integral = 0.f; // reset
-		}
-
-		return (_height_rate_error_integral > _param_vt_qc_hr_error_i.get());
+	} else {
+		_quadchute_ref_alt = -MAXFLOAT;
 	}
 
 	return false;

src/modules/vtol_att_control/vtol_type.h

@@ -288,9 +288,6 @@ 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 _height_rate_error_integral{0.f};
-
-
 	hrt_abstime _trans_finished_ts = 0;
 	hrt_abstime _transition_start_timestamp{0};
 	float _time_since_trans_start{0};
@@ -300,7 +297,8 @@ class VtolType : public ModuleParams
 
 	hrt_abstime _last_loop_ts = 0;
 	float _transition_dt = 0;
-	hrt_abstime _last_loop_quadchute_timestamp = 0;
+
+	float _quadchute_ref_alt{-MAXFLOAT};	// altitude (AMSL) reference to compute quad-chute altitude loss condition
 
 	float _accel_to_pitch_integ = 0;
 
@@ -317,7 +315,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_QC_HR_ERROR_I>) _param_vt_qc_hr_error_i,
+					(ParamFloat<px4::params::VT_QC_ALT_LOSS>) _param_vt_qc_alt_loss,
 					(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,