diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 1c213d10ca..4462868f4b 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -74,14 +74,14 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { // @Description: Zero lift drag coefficient // @Range: 0.005 0.5 // @User: Advanced - AP_GROUPINFO("POLAR_CD0", 9, SoaringController, polar_CD0, 0.027), + AP_GROUPINFO("POLAR_CD0", 9, SoaringController, _polarParams.CD0, 0.027), // @Param: POLAR_B // @DisplayName: Induced drag coeffient // @Description: Induced drag coeffient // @Range: 0.005 0.05 // @User: Advanced - AP_GROUPINFO("POLAR_B", 10, SoaringController, polar_B, 0.031), + AP_GROUPINFO("POLAR_B", 10, SoaringController, _polarParams.B, 0.031), // @Param: POLAR_K // @DisplayName: Cl factor @@ -89,7 +89,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { // @Units: m.m/s/s // @Range: 20 400 // @User: Advanced - AP_GROUPINFO("POLAR_K", 11, SoaringController, polar_K, 25.6), + AP_GROUPINFO("POLAR_K", 11, SoaringController, _polarParams.K, 25.6), // @Param: ALT_MAX // @DisplayName: Maximum soaring altitude, relative to the home location @@ -144,7 +144,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) : _tecs(tecs), - _vario(parms), + _vario(parms,_polarParams), _aparm(parms), _throttle_suppressed(true) { @@ -352,7 +352,7 @@ void SoaringController::update_cruising() void SoaringController::update_vario() { - _vario.update(thermal_bank, polar_K, polar_CD0, polar_B); + _vario.update(thermal_bank); } diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 9efd3ca110..d45849e434 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -58,6 +58,8 @@ class SoaringController { LowPassFilter _position_x_filter{1/60.0}; LowPassFilter _position_y_filter{1/60.0}; + Variometer::PolarParams _polarParams; + protected: AP_Int8 soar_active; AP_Float thermal_vspeed; @@ -67,9 +69,6 @@ protected: AP_Float thermal_distance_ahead; AP_Int16 min_thermal_s; AP_Int16 min_cruise_s; - AP_Float polar_CD0; - AP_Float polar_B; - AP_Float polar_K; AP_Float alt_max; AP_Float alt_min; AP_Float alt_cutoff; diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index 3623985cd1..63847aec88 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -6,12 +6,13 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity. #include -Variometer::Variometer(const AP_Vehicle::FixedWing &parms) : - _aparm(parms) +Variometer::Variometer(const AP_Vehicle::FixedWing &parms, PolarParams &polarParams) : + _aparm(parms), + _polarParams(polarParams) { } -void Variometer::update(const float thermal_bank, const float polar_K, const float polar_Cd0, const float polar_B) +void Variometer::update(const float thermal_bank) { const AP_AHRS &_ahrs = AP::ahrs(); @@ -26,7 +27,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo float aspd_filt = _sp_filter.apply(aspd); // Constrained airspeed. - const float minV = sqrtf(polar_K/1.5); + const float minV = sqrtf(_polarParams.K/1.5); _aspd_filt_constrained = aspd_filt>minV ? aspd_filt : minV; tau = calculate_circling_time_constant(radians(thermal_bank)); @@ -62,7 +63,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo // Compute still-air sinkrate float roll = _ahrs.roll; - float sinkrate = calculate_aircraft_sinkrate(roll, polar_K, polar_Cd0, polar_B); + float sinkrate = calculate_aircraft_sinkrate(roll); reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate; @@ -73,7 +74,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo _prev_update_time = AP_HAL::micros64(); - _expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank), polar_K, polar_Cd0, polar_B); + _expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank)); // @LoggerMessage: VAR // @Vehicles: Plane @@ -106,19 +107,16 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo } -float Variometer::calculate_aircraft_sinkrate(float phi, - const float polar_K, - const float polar_CD0, - const float polar_B) const +float Variometer::calculate_aircraft_sinkrate(float phi) const { // 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 - CL0 = polar_K / (_aspd_filt_constrained * _aspd_filt_constrained); + CL0 = _polarParams.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 + C1 = _polarParams.CD0 / CL0; // constant describing expected angle to overcome zero-lift drag + C2 = _polarParams.B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank float cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi) diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index db571f0856..20f08313d3 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -44,13 +44,20 @@ class Variometer { LowPassFilter _vdotbias_filter{1/60.0}; public: - Variometer(const AP_Vehicle::FixedWing &parms); + struct PolarParams { + AP_Float K; + AP_Float CD0; + AP_Float B; + }; + + Variometer(const AP_Vehicle::FixedWing &parms, PolarParams &polarParams); + float alt; float reading; 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 update(const float thermal_bank); + float calculate_aircraft_sinkrate(float phi) const; void reset_climb_filter(float value) { _climb_filter.reset(value);} @@ -67,5 +74,8 @@ public: float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;}; float calculate_circling_time_constant(const float thermal_bank); + +private: + PolarParams &_polarParams; };