forked from Archive/PX4-Autopilot
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>
This commit is contained in:
parent
ed0d26de8a
commit
b60e73c76f
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -316,7 +316,7 @@ protected:
|
|||
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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue