From d404cc6542f678f47e9cfd4395f9823b7fd018aa Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 12 Nov 2014 11:26:28 +1100
Subject: [PATCH] AP_Baro: add set_external_temperature()

this allows the use of an external temperature sensor for calibration
purposes, such as the sensor built in to the digital airspeed sensor.

The main affect this has is on the EAS2TAS calculation

The get_calibration_temperature() is used to choose either an external
temperature or an internal one. If an internal one is used then it is
clamped at no higher than 25 degrees C, to prevent hot electronics
on startup affecting altitude scaling and EAS2TAS
---
 libraries/AP_Baro/AP_Baro.cpp | 44 +++++++++++++++++++++++++++++++----
 libraries/AP_Baro/AP_Baro.h   | 14 ++++++++++-
 2 files changed, 53 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 20b05e37e3..97697f4a3e 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -76,7 +76,7 @@ void AP_Baro::calibrate()
                         "for more than 500ms in AP_Baro::calibrate [1]\r\n"));
             }
             ground_pressure         = get_pressure();
-            ground_temperature      = get_temperature();
+            ground_temperature      = get_calibration_temperature();
             hal.scheduler->delay(20);
         }
     }
@@ -93,7 +93,7 @@ void AP_Baro::calibrate()
             }
         } while (!_flags.healthy);
         ground_pressure     = get_pressure();
-        ground_temperature  = get_temperature();
+        ground_temperature  = get_calibration_temperature();
 
         hal.scheduler->delay(100);
     }
@@ -111,7 +111,7 @@ void AP_Baro::calibrate()
         } while (!_flags.healthy);
         ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f);
         ground_temperature = (ground_temperature * 0.8f) + 
-            (get_temperature() * 0.2f);
+            (get_calibration_temperature() * 0.2f);
 
         hal.scheduler->delay(100);
     }
@@ -129,7 +129,13 @@ void AP_Baro::update_calibration()
 {
     float pressure = get_pressure();
     _ground_pressure.set(pressure);
-    _ground_temperature.set(get_temperature());
+    float last_temperature = _ground_temperature;
+    _ground_temperature.set(get_calibration_temperature());
+    if (fabsf(last_temperature - _ground_temperature) > 3) {
+        // reset _EAS2TAS to force it to recalculate. This happens
+        // when a digital airspeed sensor comes online
+        _EAS2TAS = 0;
+    }
 }
 
 // return altitude difference in meters between current pressure and a
@@ -215,3 +221,33 @@ float AP_Baro::get_climb_rate(void)
     return _climb_rate_filter.slope() * 1.0e3f;
 }
 
+
+/*
+  set external temperature to be used for calibration (degrees C)
+ */
+void AP_Baro::set_external_temperature(float temperature)
+{
+    _external_temperature = temperature;
+    _last_external_temperature_ms = hal.scheduler->millis();
+}
+
+/*
+  get the temperature in degrees C to be used for calibration purposes
+ */
+float AP_Baro::get_calibration_temperature(void) const
+{
+    // if we have a recent external temperature then use it
+    if (_last_external_temperature_ms != 0 && hal.scheduler->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
+    // 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
+    float ret = get_temperature();
+    if (ret > 25) {
+        ret = 25;
+    }
+    return ret;    
+}
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 43859b71dd..3357695cbb 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -16,7 +16,8 @@ public:
         _altitude(0.0f),
         _last_altitude_EAS2TAS(0.0f),
         _EAS2TAS(0.0f),
-        _last_altitude_t(0)
+        _last_altitude_t(0),
+        _last_external_temperature_ms(0)
     {
         // initialise flags
         _flags.healthy = false;
@@ -82,6 +83,11 @@ public:
         return _ground_pressure.get();
     }
 
+    // set the temperature to be used for altitude calibration. This
+    // allows an external temperature source (such as a digital
+    // airspeed sensor) to be used as the temperature source
+    void set_external_temperature(float temperature);
+
     // get last time sample was taken (in ms)
     uint32_t        get_last_update() const { return _last_update; };
 
@@ -98,12 +104,18 @@ protected:
     uint8_t                             _pressure_samples;
 
 private:
+    // get the temperature to be used for altitude calibration
+    float                               get_calibration_temperature(void) const;
+
+
     AP_Float                            _ground_temperature;
     AP_Float                            _ground_pressure;
     AP_Int8                             _alt_offset;
     float                               _altitude;
     float                               _last_altitude_EAS2TAS;
     float                               _EAS2TAS;
+    float                               _external_temperature;
+    uint32_t                            _last_external_temperature_ms;
     uint32_t                            _last_altitude_t;
     DerivativeFilterFloat_Size7         _climb_rate_filter;
 };