bad descend quadchute: take altitude reset into account (#22643)

- apply delta from reset to reference altitude state to avoid false triggering

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

src/modules/vtol_att_control/vtol_type.cpp

@@ -286,13 +286,17 @@ bool VtolType::isUncommandedDescent()
 	    && (current_altitude < _tecs_status->altitude_reference)
 	    && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
 
+		if (!PX4_ISFINITE(_quadchute_ref_alt)) {
+			_quadchute_ref_alt = current_altitude;
+		}
+
 		_quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude),
 					       _tecs_status->altitude_reference);
 
 		return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get();
 
 	} else {
-		_quadchute_ref_alt = -MAXFLOAT;
+		_quadchute_ref_alt = NAN;
 	}
 
 	return false;
@@ -318,6 +322,11 @@ void VtolType::handleEkfResets()
 	if (_local_pos->z_reset_counter != _altitude_reset_counter) {
 		_local_position_z_start_of_transition += _local_pos->delta_z;
 		_altitude_reset_counter = _local_pos->z_reset_counter;
+
+		if (PX4_ISFINITE(_quadchute_ref_alt)) {
+			_quadchute_ref_alt += _local_pos->delta_z;
+		}
+
 	}
 }
 

src/modules/vtol_att_control/vtol_type.h

@@ -316,7 +316,7 @@ class VtolType : public ModuleParams
 	hrt_abstime _last_loop_ts = 0;
 	float _transition_dt = 0;
 
-	float _quadchute_ref_alt{-MAXFLOAT};	// altitude (AMSL) reference to compute quad-chute altitude loss condition
+	float _quadchute_ref_alt{NAN};	// altitude (AMSL) reference to compute quad-chute altitude loss condition
 
 	float _accel_to_pitch_integ = 0;