diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 2221bcc3c9..00b46fa84b 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -172,9 +172,10 @@ bool SoaringController::suppress_throttle() _spdHgt.reset_pitch_I(); _cruise_start_time_us = AP_HAL::micros64(); + // Reset the filtered vario rate - it is currently elevated due to the climb rate and would otherwise take a while to fall again, // leading to false positives. - _vario.filtered_reading = 0; + _vario.reset_trigger_filter(0.0f); } return _throttle_suppressed; @@ -186,7 +187,7 @@ bool SoaringController::check_thermal_criteria() return (status == ActiveStatus::AUTO_MODE_CHANGE && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6)) - && (_vario.filtered_reading - _vario.get_exp_thermalling_sink()) > thermal_vspeed + && (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed && _vario.alt < alt_max && _vario.alt > alt_min); } @@ -217,7 +218,7 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2 const float mcCreadyAlt = McCready(alt); if (_thermalability < mcCreadyAlt) { result = LoiterStatus::THERMAL_WEAK; - } else if (alt < (-_thermal_start_pos.z) || _vario.smoothed_climb_rate < 0.0) { + } else if (alt < (-_thermal_start_pos.z) || _vario.get_filtered_climb() < 0.0) { result = LoiterStatus::ALT_LOST; } else if (check_drift(prev_wp, next_wp)) { result = LoiterStatus::DRIFT_EXCEEDED; @@ -270,7 +271,7 @@ void SoaringController::init_thermalling() _thermal_start_time_us = AP_HAL::micros64(); _thermal_start_pos = position; - _vario.reset_filter(0.0); + _vario.reset_climb_filter(0.0); _position_x_filter.reset(_ekf.X[2]); _position_y_filter.reset(_ekf.X[3]); @@ -298,7 +299,7 @@ void SoaringController::update_thermalling() return; } - Vector3f wind_drift = _ahrs.wind_estimate()*deltaT*_vario.smoothed_climb_rate/_ekf.X[0]; + Vector3f wind_drift = _ahrs.wind_estimate()*deltaT*_vario.get_filtered_climb()/_ekf.X[0]; // update the filter _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y); diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 0de5b47fb4..16b67f369f 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -118,7 +118,7 @@ public: float get_vario_reading() const { - return _vario.displayed_reading; + return _vario.get_displayed_value(); } void update_vario(); diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index e5e5452b0e..0f81e77e42 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -22,12 +22,12 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo if (!_ahrs.airspeed_estimate(aspd)) { aspd = _aparm.airspeed_cruise_cm / 100.0f; } - _aspd_filt = _sp_filter.apply(aspd); + + float aspd_filt = _sp_filter.apply(aspd); // Constrained airspeed. const float minV = sqrtf(polar_K/1.5); - _aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV; - + _aspd_filt_constrained = aspd_filt>minV ? aspd_filt : minV; tau = calculate_circling_time_constant(radians(thermal_bank)); @@ -43,20 +43,22 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo float dsp = _vdot_filter.apply(temp); // Now we need to high-pass this signal to remove bias. - _vdot_filter2.set_cutoff_frequency(1/(20*tau)); - float dsp_bias = _vdot_filter2.apply(temp, dt); + _vdotbias_filter.set_cutoff_frequency(1/(20*tau)); + float dsp_bias = _vdotbias_filter.apply(temp, dt); float dsp_cor = dsp - dsp_bias; Vector3f velned; + + float raw_climb_rate = 0.0f; if (_ahrs.get_velocity_NED(velned)) { // if possible use the EKF vertical velocity raw_climb_rate = -velned.z; } _climb_filter.set_cutoff_frequency(1/(3*tau)); - smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt); + float smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt); // Compute still-air sinkrate float roll = _ahrs.roll; @@ -65,8 +67,9 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate; - filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise - displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading; + float filtered_reading = _trigger_filter.apply(reading, dt); // Apply low pass timeconst filter for noise + + _audio_filter.apply(reading, dt); // Apply low pass timeconst filter for noise _prev_update_time = AP_HAL::micros64(); @@ -95,7 +98,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo (double)roll, (double)reading, (double)filtered_reading, - (double)raw_climb_rate, + (double)_raw_climb_rate, (double)smoothed_climb_rate, (double)_expected_thermalling_sink, (double)dsp, diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index 5070ca90ee..3010630142 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -10,10 +10,6 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity. #include #include -#define ASPD_FILT 0.05 -#define TE_FILT 0.03 -#define TE_FILT_DISPLAYED 0.15 - class Variometer { const AP_Vehicle::FixedWing &_aparm; @@ -21,7 +17,8 @@ class Variometer { // store time of last update uint64_t _prev_update_time; - float _aspd_filt; + float _raw_climb_rate; + float _aspd_filt_constrained; float _expected_thermalling_sink; @@ -31,26 +28,41 @@ class Variometer { AverageFilterFloat_Size5 _sp_filter; - // low pass filter @ 30s time constant + /* + low pass filters for various purposes. + */ + // Climb rate filter for monitoring progress in thermal. LowPassFilter _climb_filter{1/60.0}; - LowPassFilter _vdot_filter2{1/60.0}; + + // Fast filter for mavlink/audio vario output. + LowPassFilter _audio_filter{1/0.71}; + + // Slower filter for deciding to enter THERMAL mode. + LowPassFilter _trigger_filter{1/4.06}; + + // Longitudinal acceleration bias filter. + LowPassFilter _vdotbias_filter{1/60.0}; public: Variometer(const AP_Vehicle::FixedWing &parms); float alt; float reading; - float filtered_reading; - float displayed_reading; - float raw_climb_rate; - float smoothed_climb_rate; float tau; void update(const float thermal_bank, const float polar_K, const float polar_CD0, const float polar_B); float calculate_aircraft_sinkrate(float phi, const float polar_K, const float polar_CD0, const float polar_B) const; - void reset_filter(float value) { _climb_filter.reset(value);} + void reset_climb_filter(float value) { _climb_filter.reset(value);} - float get_airspeed(void) const {return _aspd_filt;}; + void reset_trigger_filter(float value) { _trigger_filter.reset(value);} + + float get_airspeed(void) const {return _aspd_filt_constrained;}; + + float get_displayed_value(void) const {return _audio_filter.get();}; + + float get_filtered_climb(void) const {return _climb_filter.get();}; + + float get_trigger_value(void) const {return _trigger_filter.get();}; float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};