AP_Baro: improved barometer averaging

this changes the barometer calculations to floating point. On a MS5611
this is actually about twice as fast as the previous 64 bit
calculations, but gains us more accuracy as we are able to take
advantage of sub-bit precision when we average over 8 samples.

libraries/AP_Baro/AP_Baro.cpp

@@ -14,17 +14,20 @@
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
+    // NOTE: Index numbers 0 and 1 were for the old integer
+    // ground temperature and pressure
+
 	// @Param: ABS_PRESS
 	// @DisplayName: Absolute Pressure
 	// @Description: calibrated ground pressure
 	// @Increment: 1
-    AP_GROUPINFO("ABS_PRESS", 0, AP_Baro, _ground_pressure),
+    AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, _ground_pressure),
 
 	// @Param: ABS_PRESS
 	// @DisplayName: ground temperature
 	// @Description: calibrated ground temperature
 	// @Increment: 1
-    AP_GROUPINFO("TEMP", 1, AP_Baro, _ground_temperature),
+    AP_GROUPINFO("TEMP", 3, AP_Baro, _ground_temperature),
     AP_GROUPEND
 };
 
@@ -32,8 +35,8 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
 // the altitude() or climb_rate() interfaces can be used
 void AP_Baro::calibrate(void (*callback)(unsigned long t))
 {
-	int32_t ground_pressure = 0;
-	int16_t ground_temperature;
+	float ground_pressure = 0;
+	float ground_temperature = 0;
 
 	while (ground_pressure == 0 || !healthy) {
 		read(); // Get initial data from absolute pressure sensor
@@ -41,14 +44,27 @@ void AP_Baro::calibrate(void (*callback)(unsigned long t))
 		ground_temperature 	= get_temperature();
 		callback(20);
 	}
+    // let the barometer settle for a full second after startup
+    // the MS5611 reads quite a long way off for the first second,
+    // leading to about 1m of error if we don't wait    
+	for (uint16_t i = 0; i < 10; i++) {
+		do {
+			read();
+		} while (!healthy);
+		ground_pressure 	= get_pressure();
+		ground_temperature 	= get_temperature();
+		callback(100);
+    }
 
-	for (int i = 0; i < 30; i++) {
+    // now average over 5 values for the ground pressure and
+    // temperature settings    
+	for (uint16_t i = 0; i < 5; i++) {
 		do {
 			read();
 		} while (!healthy);
-		ground_pressure		= (ground_pressure * 9l   + get_pressure()) / 10l;
-		ground_temperature	= (ground_temperature * 9 + get_temperature()) / 10;
-		callback(20);
+		ground_pressure		= ground_pressure * 0.8     + get_pressure() * 0.2;
+		ground_temperature	= ground_temperature * 0.8  + get_temperature() * 0.2;
+		callback(100);
 	}
 
 	_ground_pressure.set_and_save(ground_pressure);
@@ -75,6 +91,10 @@ float AP_Baro::get_altitude(void)
 	_altitude = log(scaling) * temp * 29.271267f;
 
     _last_altitude_t = _last_update;
+
+    // ensure the climb rate filter is updated
+    _climb_rate_filter.update(_altitude, _last_update);
+
     return _altitude;
 }
 
@@ -83,15 +103,15 @@ float AP_Baro::get_altitude(void)
 // note that this relies on read() being called regularly to get new data
 float AP_Baro::get_climb_rate(void)
 {
-    if (_last_climb_rate_t == _last_update) {
+    if (_last_climb_rate_t == _last_altitude_t) {
         // no new information
         return _climb_rate;
     }
-    _last_climb_rate_t = _last_update;
+    _last_climb_rate_t = _last_altitude_t;
 
-    // we use a 9 point derivative filter on the climb rate. This seems
+    // we use a 7 point derivative filter on the climb rate. This seems
     // to produce somewhat reasonable results on real hardware
-    _climb_rate = _climb_rate_filter.apply(get_altitude(), _last_update) * 1.0e3;
+    _climb_rate = _climb_rate_filter.slope() * 1.0e3;
 
 	return _climb_rate;
 }

libraries/AP_Baro/AP_Baro.h

@@ -15,8 +15,8 @@ class AP_Baro
 	AP_Baro() {}
 	virtual bool    init(AP_PeriodicProcess *scheduler)=0;
 	virtual uint8_t read() = 0;
-	virtual int32_t get_pressure() = 0;
-	virtual int16_t get_temperature() = 0;
+	virtual float get_pressure() = 0;
+	virtual float get_temperature() = 0;
 	
 	virtual int32_t get_raw_pressure() = 0;
 	virtual int32_t get_raw_temp() = 0;
@@ -39,8 +39,8 @@ class AP_Baro
     float get_climb_rate(void);
 
     // the ground values are only valid after calibration
-    int16_t get_ground_temperature(void) { return _ground_temperature.get(); }
-    int32_t get_ground_pressure(void) { return _ground_pressure.get(); }
+    float get_ground_temperature(void) { return _ground_temperature.get(); }
+    float get_ground_pressure(void) { return _ground_pressure.get(); }
 
 	static const struct AP_Param::GroupInfo var_info[];
 
@@ -49,8 +49,8 @@ protected:
     uint8_t _pressure_samples;
 
 private:
-    AP_Int16    _ground_temperature;
-    AP_Int32    _ground_pressure;
+    AP_Float    _ground_temperature;
+    AP_Float    _ground_pressure;
     float       _altitude;
     float       _climb_rate;
     uint32_t    _last_climb_rate_t;

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -128,11 +128,11 @@ uint8_t AP_Baro_BMP085::read()
 	return(result);
 }
 
-int32_t AP_Baro_BMP085::get_pressure() {
+float AP_Baro_BMP085::get_pressure() {
     return Press;
 }
 
-int16_t AP_Baro_BMP085::get_temperature() {
+float AP_Baro_BMP085::get_temperature() {
     return Temp;
 }
 

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -19,8 +19,8 @@ class AP_Baro_BMP085 : public AP_Baro
     /* AP_Baro public interface: */
     bool    init(AP_PeriodicProcess * scheduler);
     uint8_t read();
-    int32_t get_pressure();
-    int16_t get_temperature();
+    float get_pressure();
+    float get_temperature();
 
     int32_t get_raw_pressure();
     int32_t get_raw_temp();

libraries/AP_Baro/AP_Baro_BMP085_hil.cpp

@@ -25,8 +25,8 @@ uint8_t AP_Baro_BMP085_HIL::read()
 
     if (_count != 0) {
         result = 1;
-        Press = _pressure_sum / _count;
-        Temp = _temperature_sum / _count;
+        Press = ((float)_pressure_sum) / _count;
+        Temp = ((float)_temperature_sum) / _count;
         _pressure_samples = _count;
         _count = 0;
         _pressure_sum = 0;
@@ -54,11 +54,11 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
     _last_update = millis();
 }
 
-int32_t AP_Baro_BMP085_HIL::get_pressure() {
+float AP_Baro_BMP085_HIL::get_pressure() {
     return Press;
 }
 
-int16_t AP_Baro_BMP085_HIL::get_temperature() {
+float AP_Baro_BMP085_HIL::get_temperature() {
     return Temp;
 }
 

libraries/AP_Baro/AP_Baro_BMP085_hil.h

@@ -9,8 +9,8 @@ class AP_Baro_BMP085_HIL : public AP_Baro
 {
 private:
 	uint8_t BMP085_State;
-    int32_t Temp;
-    int32_t Press;
+    float Temp;
+    float Press;
     int32_t _pressure_sum;
     int32_t _temperature_sum;
     uint8_t _count;
@@ -20,8 +20,8 @@ public:
 
 	bool init(AP_PeriodicProcess * scheduler);
 	uint8_t read();
-	int32_t get_pressure();
-	int16_t get_temperature();
+	float get_pressure();
+	float get_temperature();
 	int32_t get_raw_pressure();
 	int32_t get_raw_temp();
 	void setHIL(float Temp, float Press);

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -18,7 +18,7 @@
 
 	Methods:
 		init() : Initialization and sensor reset
-		read() : Read sensor data and _calculate Temperature, Pressure and Altitude
+		read() : Read sensor data and _calculate Temperature, Pressure
 		         This function is optimized so the main host don´t need to wait
 				 You can call this function in your main loop
 				 Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
@@ -221,10 +221,10 @@ uint8_t AP_Baro_MS5611::read()
         _updated = false;
         sei();
         if (d1count != 0) {
-            D1 = sD1 / d1count;
+            D1 = ((float)sD1) / d1count;
         }
         if (d2count != 0) {
-            D2 = sD2 / d2count;
+            D2 = ((float)sD2) / d2count;
         }
         _pressure_samples = d1count;
         _raw_press = D1;
@@ -241,46 +241,49 @@ uint8_t AP_Baro_MS5611::read()
 // Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
 void AP_Baro_MS5611::_calculate()
 {
-	int32_t dT;
-	int64_t TEMP;  // 64 bits
-	int64_t OFF;
-	int64_t SENS;
-	int64_t P;
+	float dT;
+	float TEMP;
+	float OFF;
+	float SENS;
+	float P;
 
 	// Formulas from manufacturer datasheet
-	// as per data sheet some intermediate results require over 32 bits, therefore
-  // we define parameters as 64 bits to prevent overflow on operations
-  // sub -20c temperature compensation is not included
-    // Serial.printf("D1=%lu D2=%lu\n", (unsigned long)D1, (unsigned long)D2);
-	dT = D2-((int32_t)C5*256);
-	TEMP = 2000 + ((int64_t)dT * C6)/8388608;
-	OFF = (int64_t)C2 * 65536 + ((int64_t)C4 * dT ) / 128;
-	SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
-
-	if (TEMP < 2000){   // second order temperature compensation
-		int64_t T2 = (((int64_t)dT)*dT) >> 31;
-		int64_t Aux_64 = (TEMP-2000)*(TEMP-2000);
-		int64_t OFF2 = (5*Aux_64)>>1;
-		int64_t SENS2 = (5*Aux_64)>>2;
+    // sub -20c temperature compensation is not included
+
+    // we do the calculations using floating point
+    // as this is much faster on an AVR2560, and also allows
+    // us to take advantage of the averaging of D1 and D1 over
+    // multiple samples, giving us more precision
+	dT = D2-(((uint32_t)C5)<<8);
+	TEMP = (dT * C6)/8388608;
+	OFF = C2 * 65536.0 + (C4 * dT) / 128;
+	SENS = C1 * 32768.0 + (C3 * dT) / 256;
+
+	if (TEMP < 0) {
+        // second order temperature compensation when under 20 degrees C
+		float T2 = (dT*dT) / 0x80000000;
+		float Aux = TEMP*TEMP;
+		float OFF2 = 2.5*Aux;
+		float SENS2 = 1.25*Aux;
 		TEMP = TEMP - T2;
 		OFF = OFF - OFF2;
 		SENS = SENS - SENS2;
 	}
 
 	P = (D1*SENS/2097152 - OFF)/32768;
-	Temp = TEMP;
+	Temp = TEMP + 2000;
 	Press = P;
 }
 
-int32_t AP_Baro_MS5611::get_pressure()
+float AP_Baro_MS5611::get_pressure()
 {
-	return(Press);
+	return Press;
 }
 
-int16_t AP_Baro_MS5611::get_temperature()
+float AP_Baro_MS5611::get_temperature()
 {
 	// callers want the temperature in 0.1C units
-	return(Temp/10);
+	return Temp/10;
 }
 
 int32_t AP_Baro_MS5611::get_raw_pressure() {

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -13,12 +13,14 @@ class AP_Baro_MS5611 : public AP_Baro
   /* AP_Baro public interface: */
   bool init(AP_PeriodicProcess *scheduler);
   uint8_t read();
-  int32_t get_pressure();     // in mbar*100 units
-  int16_t get_temperature();  // in celsius degrees * 100 units
+  float get_pressure();     // in mbar*100 units
+  float get_temperature();  // in celsius degrees * 100 units
 
   int32_t get_raw_pressure();
   int32_t get_raw_temp();
 
+  void     _calculate();
+
   private:
   /* Asynchronous handler functions: */ 
   static void _update(uint32_t );
@@ -38,17 +40,15 @@ class AP_Baro_MS5611 : public AP_Baro
   static uint32_t _spi_read_adc();
   static void     _spi_write(uint8_t reg);
 
-  void     _calculate();
 
-  int16_t Temp;
-  int32_t Press;
-  int32_t Alt;
+  float Temp;
+  float Press;
 
   int32_t _raw_press;
   int32_t _raw_temp;
   // Internal calibration registers
   uint16_t C1,C2,C3,C4,C5,C6;
-  uint32_t D1,D2;
+  float D1,D2;
 };
 
 #endif //  __AP_BARO_MS5611_H__