Baro: added get_altitude() and get_climb_rate() interfaces

this allows the barometer driver to calibrate and return altitude and
climb rate values. This will be used by the AHRS drift correction code
for vertical velocity

The climb rate uses a 5 point average filter

libraries/AP_Baro/AP_Baro.cpp

@@ -0,0 +1,95 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+	APM_Baro.cpp - barometer driver
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public License
+    as published by the Free Software Foundation; either version 2.1
+    of the License, or (at your option) any later version.
+*/
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Baro.h>
+
+// calibrate the barometer. This must be called at least once before
+// 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;
+
+	while (ground_pressure == 0 || !healthy) {
+		read(); // Get initial data from absolute pressure sensor
+		ground_pressure 	= get_pressure();
+		ground_temperature 	= get_temperature();
+		callback(20);
+	}
+
+	for (int i = 0; i < 30; 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.set_and_save(ground_pressure);
+	_ground_temperature.set_and_save(ground_temperature / 10.0f);
+}
+
+// return current altitude estimate relative to time that calibrate()
+// was called. Returns altitude in meters
+// note that this relies on read() being called regularly to get new data
+float AP_Baro::get_altitude(void)
+{
+	float scaling, temp;
+
+    if (_last_altitude_t == _last_update) {
+        // no new information
+        return _altitude;
+    }
+
+    // this has no filtering of the pressure values, use a separate
+    // filter if you want a smoothed value. The AHRS driver wants
+    // unsmoothed values
+	scaling 				= (float)_ground_pressure / (float)get_pressure();
+	temp 					= ((float)_ground_temperature) + 273.15f;
+	_altitude = log(scaling) * temp * 29.271267f;
+
+    _last_altitude_t = _last_update;
+    return _altitude;
+}
+
+// return current climb_rate estimeate relative to time that calibrate()
+// was called. Returns climb rate in meters/s, positive means up
+// note that this relies on read() being called regularly to get new data
+float AP_Baro::get_climb_rate(void)
+{
+    float alt, deltat;
+
+    if (_last_climb_rate_t == _last_update) {
+        // no new information
+        return _climb_rate;
+    }
+    if (_last_climb_rate_t == 0) {
+        // first call
+        _last_altitude = get_altitude();
+        _last_climb_rate_t = _last_update;
+        _climb_rate = 0.0;
+        return _climb_rate;
+    }
+
+    deltat = (_last_update - _last_climb_rate_t) * 1.0e-3;
+
+    alt = get_altitude();
+
+    // we use a 5 point average filter on the climb rate. This seems
+    // to produce somewhat reasonable results on real hardware
+    _climb_rate = _climb_rate_filter.apply((alt - _last_altitude) / deltat);
+    _last_altitude = alt;
+    _last_climb_rate_t = _last_update;
+
+	return _climb_rate;
+}

libraries/AP_Baro/AP_Baro.h

@@ -3,6 +3,9 @@
 #ifndef __AP_BARO_H__
 #define __AP_BARO_H__
 
+#include <AP_Param.h>
+#include <Filter.h>
+#include <AverageFilter.h>
 #include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
 
 class AP_Baro
@@ -14,10 +17,39 @@ class AP_Baro
 	virtual uint8_t read() = 0;
 	virtual int32_t get_pressure() = 0;
 	virtual int16_t get_temperature() = 0;
-	virtual float   get_altitude() = 0;
 	
 	virtual int32_t get_raw_pressure() = 0;
 	virtual int32_t get_raw_temp() = 0;
+
+    // calibrate the barometer. This must be called on startup if the
+    // altitude/climb_rate/acceleration interfaces are ever used
+    // the callback is a delay() like routine
+    void calibrate(void (*callback)(unsigned long t));
+
+    // get current altitude in meters relative to altitude at the time
+    // of the last calibrate() call
+    float get_altitude(void);
+
+    // get current climb rate in meters/s. A positive number means
+    // going up
+    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(); }
+
+protected:
+    uint32_t _last_update;
+
+private:
+    AP_Int16    _ground_temperature;
+    AP_Int32    _ground_pressure;
+    float       _altitude;
+    uint32_t    _last_altitude_t;
+    float		_last_altitude;
+    float       _climb_rate;
+    uint32_t    _last_climb_rate_t;
+    AverageFilterFloat_Size5 _climb_rate_filter;
 };
 
 #include "AP_Baro_MS5611.h"

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -122,6 +122,9 @@ uint8_t AP_Baro_BMP085::read()
 			result = 1;					// New pressure reading
 		}
 	}
+    if (result) {
+        _last_update = millis();
+    }
 	return(result);
 }
 
@@ -133,10 +136,6 @@ int16_t AP_Baro_BMP085::get_temperature() {
     return Temp;
 }
 
-float AP_Baro_BMP085::get_altitude() {
-    return 0.0; // TODO
-}
-
 int32_t AP_Baro_BMP085::get_raw_pressure() {
     return RawPress;
 }

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -19,7 +19,6 @@ class AP_Baro_BMP085 : public AP_Baro
     uint8_t read();
     int32_t get_pressure();
     int16_t get_temperature();
-    float   get_altitude();
 
     int32_t get_raw_pressure();
     int32_t get_raw_temp();

libraries/AP_Baro/AP_Baro_BMP085_hil.cpp

@@ -53,6 +53,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
 	Temp 	= _Temp;
 	Press 	= _Press;
 	healthy = true;
+    _last_update = millis();
 }
 
 int32_t AP_Baro_BMP085_HIL::get_pressure() {
@@ -63,10 +64,6 @@ int16_t AP_Baro_BMP085_HIL::get_temperature() {
     return Temp;
 }
 
-float AP_Baro_BMP085_HIL::get_altitude() {
-    return 0.0; // TODO
-}
-
 int32_t AP_Baro_BMP085_HIL::get_raw_pressure() {
     return Press;
 }

libraries/AP_Baro/AP_Baro_BMP085_hil.h

@@ -5,7 +5,7 @@
 
 #include "AP_Baro.h"
 
-class AP_Baro_BMP085_HIL
+class AP_Baro_BMP085_HIL : public AP_Baro
 {
 private:
 	uint8_t BMP085_State;
@@ -15,13 +15,11 @@ private:
 public:
     AP_Baro_BMP085_HIL();  // Constructor
 	//int Altitude;
-    bool healthy;
 
 	bool init(AP_PeriodicProcess * scheduler);
 	uint8_t read();
 	int32_t get_pressure();
 	int16_t get_temperature();
-	float   get_altitude();
 	int32_t get_raw_pressure();
 	int32_t get_raw_temp();
 	void setHIL(float Temp, float Press);

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -25,7 +25,6 @@
 				 It returns a 1 if there are new data.
 		get_pressure() : return pressure in mbar*100 units
 		get_temperature() : return temperature in celsius degrees*100 units
-		get_altitude() : return altitude in meters
 
 	Internal functions:
 		_calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
@@ -61,11 +60,10 @@ bool     AP_Baro_MS5611::_updated;
 
 uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
 {
-  uint8_t dump;
   uint8_t return_value;
   uint8_t addr = reg; // | 0x80; // Set most significant bit
   digitalWrite(MS5611_CS, LOW);
-  dump = SPI.transfer(addr);
+  SPI.transfer(addr); // discarded
   return_value = SPI.transfer(0);
   digitalWrite(MS5611_CS, HIGH);
   return return_value;
@@ -73,11 +71,11 @@ uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
 
 uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
 {
-  uint8_t dump, byteH, byteL;
+  uint8_t byteH, byteL;
   uint16_t return_value;
   uint8_t addr = reg; // | 0x80; // Set most significant bit
   digitalWrite(MS5611_CS, LOW);
-  dump = SPI.transfer(addr);
+  SPI.transfer(addr); // discarded
   byteH = SPI.transfer(0);
   byteL = SPI.transfer(0);
   digitalWrite(MS5611_CS, HIGH);
@@ -87,11 +85,11 @@ uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
 
 uint32_t AP_Baro_MS5611::_spi_read_adc()
 {
-  uint8_t dump,byteH,byteM,byteL;
+  uint8_t byteH,byteM,byteL;
   uint32_t return_value;
   uint8_t addr = 0x00;
   digitalWrite(MS5611_CS, LOW);
-  dump = SPI.transfer(addr);
+  SPI.transfer(addr); // discarded
   byteH = SPI.transfer(0);
   byteM = SPI.transfer(0);
   byteL = SPI.transfer(0);
@@ -103,9 +101,8 @@ uint32_t AP_Baro_MS5611::_spi_read_adc()
 
 void AP_Baro_MS5611::_spi_write(uint8_t reg)
 {
-  uint8_t dump;
   digitalWrite(MS5611_CS, LOW);
-  dump = SPI.transfer(reg);
+  SPI.transfer(reg); // discarded
   digitalWrite(MS5611_CS, HIGH);
 }
 
@@ -193,6 +190,9 @@ uint8_t AP_Baro_MS5611::read()
     }
     _sync_access = false;
     _calculate();
+    if (updated) {
+        _last_update = millis();
+    }
     return updated ? 1 : 0;
 }
 
@@ -240,19 +240,6 @@ int16_t AP_Baro_MS5611::get_temperature()
 	return(Temp/10);
 }
 
-// Return altitude using the standard 1013.25 mbar at sea level reference
-float AP_Baro_MS5611::get_altitude()
-{
-	float tmp_float;
-	float Altitude;
-
-	tmp_float = (Press / 101325.0);
-	tmp_float = pow(tmp_float, 0.190295);
-	Altitude = 44330.0 * (1.0 - tmp_float);
-
-	return (Altitude);
-}
-
 int32_t AP_Baro_MS5611::get_raw_pressure() {
 	return _raw_press;
 }

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -15,7 +15,6 @@ class AP_Baro_MS5611 : public AP_Baro
   uint8_t read();
   int32_t get_pressure();     // in mbar*100 units
   int16_t get_temperature();  // in celsius degrees * 100 units
-  float get_altitude();        // in meter units
 
   int32_t get_raw_pressure();
   int32_t get_raw_temp();