diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp new file mode 100644 index 0000000000..d7cff85326 --- /dev/null +++ b/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 +#include +#include + +// 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; +} diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 49b4b00075..764bc1f7e0 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -3,6 +3,9 @@ #ifndef __AP_BARO_H__ #define __AP_BARO_H__ +#include +#include +#include #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" diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 118278d248..b8c8ad6008 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/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; } diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index c60e022cbe..d706213626 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/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(); diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp index 71577b8d4a..87ef1a7ad5 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp +++ b/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; } diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.h b/libraries/AP_Baro/AP_Baro_BMP085_hil.h index bf121f2f16..cadb3813e9 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.h +++ b/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); diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 49b8f5da40..3a043e6ade 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/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; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 7671fd80c7..2da8430bc0 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/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();