AP_Baro: Limit ground temperature used for the altitude

Fix an incorrect EAS2TAS that was double counting altitude

libraries/AP_Baro/AP_Baro.cpp

@@ -35,6 +35,14 @@
 #include "AP_Baro_qflight.h"
 #include "AP_Baro_QURT.h"
 
+#define C_TO_KELVIN 273.15f
+// Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
+#define ISA_GAS_CONSTANT 287.26f
+#define ISA_LAPSE_RATE 0.0065f
+
+#define INTERNAL_TEMPERATURE_CLAMP 35.0f
+
+
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
@@ -60,7 +68,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @ReadOnly: True
     // @Volatile: True
     // @User: Advanced
-    AP_GROUPINFO("TEMP", 3, AP_Baro, sensors[0].ground_temperature, 0),
+    AP_GROUPINFO("TEMP", 3, AP_Baro, _user_ground_temperature, 0),
 
     // index 4 reserved for old AP_Int8 version in legacy FRAM
     //AP_GROUPINFO("ALT_OFFSET", 4, AP_Baro, _alt_offset, 0),
@@ -104,15 +112,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ABS_PRESS2", 9, AP_Baro, sensors[1].ground_pressure, 0),
 
-    // @Param: TEMP2
+    // Slot 10 used to be TEMP2
-    // @DisplayName: ground temperature
-    // @Description: calibrated ground temperature in degrees Celsius
-    // @Units: degrees celsius
-    // @Increment: 1
-    // @ReadOnly: True
-    // @Volatile: True
-    // @User: Advanced
-    AP_GROUPINFO("TEMP2", 10, AP_Baro, sensors[1].ground_temperature, 0),
 #endif
 
 #if BARO_MAX_INSTANCES > 2
@@ -126,15 +126,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ABS_PRESS3", 11, AP_Baro, sensors[2].ground_pressure, 0),
 
-    // @Param: TEMP3
+    // Slot 12 used to be TEMP3
-    // @DisplayName: ground temperature
-    // @Description: calibrated ground temperature in degrees Celsius
-    // @Units: degrees celsius
-    // @Increment: 1
-    // @ReadOnly: True
-    // @Volatile: True
-    // @User: Advanced
-    AP_GROUPINFO("TEMP3", 12, AP_Baro, sensors[2].ground_temperature, 0),
 #endif
 
     AP_GROUPEND
@@ -209,11 +201,12 @@ void AP_Baro::calibrate(bool save)
         } else {
             if (save) {
                 sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
-                sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
             }
         }
     }
 
+    _guessed_ground_temperature = get_external_temperature();
+
     // panic if all sensors are not calibrated
     for (uint8_t i=0; i<_num_sensors; i++) {
         if (sensors[i].calibrated) {
@@ -234,22 +227,20 @@ void AP_Baro::update_calibration()
         if (healthy(i)) {
             sensors[i].ground_pressure.set(get_pressure(i));
         }
-        float last_temperature = sensors[i].ground_temperature;
-        sensors[i].ground_temperature.set(get_calibration_temperature(i));
 
         // don't notify the GCS too rapidly or we flood the link
         uint32_t now = AP_HAL::millis();
         if (now - _last_notify_ms > 10000) {
             sensors[i].ground_pressure.notify();
-            sensors[i].ground_temperature.notify();
             _last_notify_ms = now;
         }
-        if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
-            // reset _EAS2TAS to force it to recalculate. This happens
-            // when a digital airspeed sensor comes online
-            _EAS2TAS = 0;
-        }
     }
+
+    // always update the guessed ground temp
+    _guessed_ground_temperature = get_external_temperature();
+
+    // force EAS2TAS to recalculate
+    _EAS2TAS = 0;
 }
 
 // return altitude difference in meters between current pressure and a
@@ -257,7 +248,7 @@ void AP_Baro::update_calibration()
 float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const
 {
     float ret;
-    float temp    = get_ground_temperature() + 273.15f;
+    float temp    = get_ground_temperature() + C_TO_KELVIN;
     float scaling = pressure / base_pressure;
 
     // This is an exact calculation that is within +-2.5m of the standard
@@ -274,13 +265,16 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons
 float AP_Baro::get_EAS2TAS(void)
 {
     float altitude = get_altitude();
-    if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && !is_zero(_EAS2TAS)) {
+    if ((fabsf(altitude - _last_altitude_EAS2TAS) < 25.0f) && !is_zero(_EAS2TAS)) {
         // not enough change to require re-calculating
         return _EAS2TAS;
     }
 
-    float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
+    // only estimate lapse rate for the difference from the ground location
-    _EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
+    // provides a more consistent reading then trying to estimate a complete
+    // ISA model atmosphere
+    float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
+    _EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
     _last_altitude_EAS2TAS = altitude;
     return _EAS2TAS;
 }
@@ -288,9 +282,9 @@ float AP_Baro::get_EAS2TAS(void)
 // return air density / sea level density - decreases as altitude climbs
 float AP_Baro::get_air_density_ratio(void)
 {
-    float eas2tas = get_EAS2TAS();
+    const float eas2tas = get_EAS2TAS();
     if (eas2tas > 0.0f) {
-        return 1.0f/(sq(get_EAS2TAS()));
+        return 1.0f/(sq(eas2tas));
     } else {
         return 1.0f;
     }
@@ -309,6 +303,17 @@ float AP_Baro::get_climb_rate(void)
     return _climb_rate_filter.slope() * 1.0e3f;
 }
 
+// returns the ground temperature in degrees C, selecting either a user
+// provided one, or the internal estimate
+float AP_Baro::get_ground_temperature(void) const
+{
+    if (is_zero(_user_ground_temperature)) {
+        return _guessed_ground_temperature;
+    } else {
+        return _user_ground_temperature;
+    }
+}
+
 
 /*
   set external temperature to be used for calibration (degrees C)
@@ -322,22 +327,21 @@ void AP_Baro::set_external_temperature(float temperature)
 /*
   get the temperature in degrees C to be used for calibration purposes
  */
-float AP_Baro::get_calibration_temperature(uint8_t instance) const
+float AP_Baro::get_external_temperature(const uint8_t instance) const
 {
     // if we have a recent external temperature then use it
     if (_last_external_temperature_ms != 0 && AP_HAL::millis() - _last_external_temperature_ms < 10000) {
         return _external_temperature;
     }
     // if we don't have an external temperature then use the minimum
-    // of the barometer temperature and 25 degrees C. The reason for
+    // of the barometer temperature and 35 degrees C. The reason for
     // not just using the baro temperature is it tends to read high,
     // often 30 degrees above the actual temperature. That means the
-    // EAS2TAS tends to be off by quite a large margin
+    // EAS2TAS tends to be off by quite a large margin, as well as
-    float ret = get_temperature(instance);
+    // the calculation of altitude difference betweeen two pressures
-    if (ret > 25) {
+    // reporting a high temperature will cause the aircraft to
-        ret = 25;
+    // estimate itself as flying higher then it actually is.
-    }
+    return MIN(get_temperature(instance), INTERNAL_TEMPERATURE_CLAMP);
-    return ret;
 }
 
 
@@ -370,6 +374,12 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
  */
 void AP_Baro::init(void)
 {
+    // ensure that there isn't a previous ground temperature saved
+    if (!is_zero(_user_ground_temperature)) {
+        _user_ground_temperature.set_and_save(0.0f);
+        _user_ground_temperature.notify();
+    }
+
     if (_hil_mode) {
         drivers[0] = new AP_Baro_HIL(*this);
         _num_drivers = 1;

libraries/AP_Baro/AP_Baro.h

@@ -83,8 +83,7 @@ public:
 
     // ground temperature in degrees C
     // the ground values are only valid after calibration
-    float get_ground_temperature(void) const { return get_ground_temperature(_primary); }
+    float get_ground_temperature(void) const;
-    float get_ground_temperature(uint8_t i)  const { return sensors[i].ground_temperature.get(); }
 
     // ground pressure in Pascal
     // the ground values are only valid after calibration
@@ -103,8 +102,8 @@ public:
     // settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
-    float get_calibration_temperature(void) const { return get_calibration_temperature(_primary); }
+    float get_external_temperature(void) const { return get_external_temperature(_primary); };
-    float get_calibration_temperature(uint8_t instance) const;
+    float get_external_temperature(const uint8_t instance) const;
 
     // HIL (and SITL) interface, setting altitude
     void setHIL(float altitude_msl);
@@ -170,7 +169,6 @@ private:
         float pressure;                 // pressure in Pascal
         float temperature;              // temperature in degrees C
         float altitude;                 // calculated altitude
-        AP_Float ground_temperature;
         AP_Float ground_pressure;
     } sensors[BARO_MAX_INSTANCES];
 
@@ -184,7 +182,9 @@ private:
     uint32_t                            _last_external_temperature_ms;
     DerivativeFilterFloat_Size7         _climb_rate_filter;
     AP_Float                            _specific_gravity; // the specific gravity of fluid for an ROV 1.00 for freshwater, 1.024 for salt water
+    AP_Float                            _user_ground_temperature; // user override of the ground temperature used for EAS2TAS
     bool                                _hil_mode:1;
+    float                               _guessed_ground_temperature; // currently ground temperature estimate using our best abailable source
 
     // when did we last notify the GCS of new pressure reference?
     uint32_t                            _last_notify_ms;