aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Soaring: Cleanup variometer.

Browse Source
This commit is contained in:
Samuel Tabor 2019-06-23 08:52:38 +01:00 committed by Andrew Tridgell
parent 62a34e0f8d
commit 16b7de4029
2 changed files with 11 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Soaring/Variometer.cpp
View File

@ -49,7 +49,7 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
// Compute still-air sinkrate
float roll = _ahrs.roll;
float sinkrate = correct_netto_rate(0.0f, roll, _aspd_filt_constrained, polar_K, polar_Cd0, polar_B);
float sinkrate = calculate_aircraft_sinkrate(roll, polar_K, polar_Cd0, polar_B);
reading = raw_climb_rate + dsp*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
@ -71,37 +71,27 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
(double)smoothed_climb_rate);
float expected_roll = asinf(constrain_float(powf(_aspd_filt_constrained,2)/(GRAVITY_MSS*_aparm.loiter_radius),-1.0, 1.0));
_expected_thermalling_sink = correct_netto_rate(0.0, expected_roll, _aspd_filt_constrained, polar_K, polar_Cd0, polar_B);
_expected_thermalling_sink = calculate_aircraft_sinkrate(expected_roll, polar_K, polar_Cd0, polar_B);
}
float Variometer::correct_netto_rate(float climb_rate,
float phi,
float aspd,
const float polar_K,
const float polar_CD0,
const float polar_B)
float Variometer::calculate_aircraft_sinkrate(float phi,
const float polar_K,
const float polar_CD0,
const float polar_B)
{
// Remove aircraft sink rate
float CL0; // CL0 = 2*W/(rho*S*V^2)
float C1; // C1 = CD0/CL0
float C2; // C2 = CDi0/CL0 = B*CL0
float netto_rate;
float cosphi;
CL0 = polar_K / (aspd * aspd);
CL0 = polar_K / (_aspd_filt_constrained * _aspd_filt_constrained);
C1 = polar_CD0 / CL0; // constant describing expected angle to overcome zero-lift drag
C2 = polar_B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank
cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi)
netto_rate = climb_rate + aspd * (C1 + C2 / (cosphi * cosphi)); // effect of aircraft drag removed
// Remove acceleration effect - needs to be tested.
//float temp_netto = netto_rate;
//float dVdt = SpdHgt_Controller->get_VXdot();
//netto_rate = netto_rate + aspd*dVdt/GRAVITY_MSS;
//gcs().send_text(MAV_SEVERITY_INFO, "%f %f %f %f",temp_netto,dVdt,netto_rate,barometer.get_altitude());
return netto_rate;
float cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi)
return _aspd_filt_constrained * (C1 + C2 / (cosphi * cosphi));
}
float Variometer::calculate_circling_time_constant()

2
libraries/AP_Soaring/Variometer.h
View File

@ -50,7 +50,7 @@ public:
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);
float calculate_aircraft_sinkrate(float phi, const float polar_K, const float polar_CD0, const float polar_B);
void reset_filter(float value) { _climb_filter.reset(value);}