mirror of https://github.com/ArduPilot/ardupilot
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:
parent
43d6015811
commit
44755bf3ce
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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() {
|
||||
|
|
|
@ -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__
|
||||
|
|
Loading…
Reference in New Issue