Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Add a 60s first order filter on climb rate. If this becomes negative exit thermalling.

This commit is contained in:
Samuel Tabor 2019-06-08 13:51:00 +01:00 committed by Andrew Tridgell
parent a56b1dadb9
commit 936d4232c9
3 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -199,10 +199,10 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: W %f.3 R %f.3 th %f.1 alt %dm Mc %dm", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (int32_t)alt, (int32_t)mcCreadyAlt);
}
} else if (alt < _thermal_start_alt) {
} else if (alt < _thermal_start_alt || _vario.smoothed_climb_rate < 0.0) {
result = ALT_LOST;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Altitude lost from entry = %dm", (int32_t)_thermal_start_alt);
gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
}
}
}
@ -252,6 +252,8 @@ void SoaringController::init_thermalling()
_prev_update_time = AP_HAL::micros64();
_thermal_start_time_us = AP_HAL::micros64();
_thermal_start_alt = _vario.alt;
_vario.reset_filter(0.0);
}
void SoaringController::init_cruising()

13
libraries/AP_Soaring/Variometer.cpp
View File

@ -10,6 +10,7 @@ Variometer::Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
_ahrs(ahrs),
_aparm(parms)
{
_climb_filter = LowPassFilter<float>(1.0/60.0);
}
void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0)
@ -26,12 +27,12 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
// take 5 point moving average
float dsp = _vdot_filter.apply(temp);
float dh = 0;
Vector3f velned;
if (_ahrs.get_velocity_NED(velned)) {
// if possible use the EKF vertical velocity
dh = -velned.z;
raw_climb_rate = -velned.z;
}
smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, (AP_HAL::micros64() - _prev_update_time)/1e6);
float aspd = 0;
if (!_ahrs.airspeed_estimate(aspd)) {
@ -44,7 +45,7 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
// Compute still-air sinkrate
float sinkrate = correct_netto_rate(0.0f, roll, _aspd_filt, polar_K, polar_Cd0, polar_B);
reading = dh + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
reading = raw_climb_rate + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise
@ -52,14 +53,16 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
_prev_update_time = AP_HAL::micros64();
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff",
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc", "Qffffffff",
AP_HAL::micros64(),
(double)0.0,
(double)_aspd_filt,
(double)alt,
(double)roll,
(double)reading,
(double)filtered_reading);
(double)filtered_reading,
(double)raw_climb_rate,
(double)smoothed_climb_rate);
}

6
libraries/AP_Soaring/Variometer.h
View File

@ -34,15 +34,21 @@ class Variometer {
AverageFilterFloat_Size5 _sp_filter;
// low pass filter @ 30s time constant
LowPassFilter<float> _climb_filter;
public:
Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
float alt;
float reading;
float filtered_reading;
float displayed_reading;
float raw_climb_rate;
float smoothed_climb_rate;
void update(const float polar_K, const float polar_CD0, const float polar_B);
float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B);
void reset_filter(float value) { _climb_filter.reset(value);}
};