AP_Soaring: Store reference to glide polar params in variometer

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);
 }
 
 

libraries/AP_Soaring/AP_Soaring.h

@@ -58,6 +58,8 @@ class SoaringController {
     LowPassFilter<float> _position_x_filter{1/60.0};
     LowPassFilter<float> _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;

libraries/AP_Soaring/Variometer.cpp

@@ -6,12 +6,13 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity.
 
 #include <AP_Logger/AP_Logger.h>
 
-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)
     

libraries/AP_Soaring/Variometer.h

@@ -44,13 +44,20 @@ class Variometer {
     LowPassFilter<float> _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;
 };