Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Andrew Tridgell 2012-07-06 15:11:22 +10:00
parent 520d762382
commit f501503eb0
8 changed files with 87 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_Baro/AP_Baro.cpp
View File

@ -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);
}
for (int i = 0; i < 30; i++) {
// 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 = (ground_pressure * 9l + get_pressure()) / 10l;
ground_temperature = (ground_temperature * 9 + get_temperature()) / 10;
callback(20);
ground_pressure = get_pressure();
ground_temperature = get_temperature();
callback(100);
}
// 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 * 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;
}

12
libraries/AP_Baro/AP_Baro.h
View File

@ -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;

4
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -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;
}

4
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -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();

8
libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
View File

@ -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;
}

8
libraries/AP_Baro/AP_Baro_BMP085_hil.h
View File

@ -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);

55
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -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;
// sub -20c temperature compensation is not included
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;
// 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() {

14
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -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__