baro: fixed an integer overflow issue at high altitudes

the averaging array was using 16 bit numbers, but we are storing
numbers with 19 significant bits. That caused overflow at high
altitude, and some very interesting altitude graphs!

Thanks to Michael Oborne for spotting this in a log
This commit is contained in:
Andrew Tridgell 2012-01-14 19:50:45 +11:00
parent 4e9c668315
commit 817e7442bd
3 changed files with 15 additions and 19 deletions

View File

@ -56,6 +56,8 @@ extern "C" {
// is not available to the arduino digitalRead function.
#define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC))
// oversampling 3 gives highest resolution
#define OVERSAMPLING 3
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
@ -64,7 +66,6 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
// We read the calibration data registers
@ -142,7 +143,7 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
// Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress()
{
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) {
healthy = false;
}
}
@ -163,14 +164,15 @@ void AP_Baro_BMP085::ReadPress()
return;
}
RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
if(_offset_press == 0){
if (_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
} else{
RawPress -= _offset_press;
}
// filter
_press_filter[_press_index++] = RawPress;
@ -186,7 +188,6 @@ void AP_Baro_BMP085::ReadPress()
// grab result
RawPress /= PRESS_FILTER_SIZE;
//RawPress >>= 3;
RawPress += _offset_press;
}
@ -261,16 +262,16 @@ void AP_Baro_BMP085::Calculate()
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11;
x3 = x1 + x2;
//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0
//b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0
tmp = ac1;
tmp = (tmp*4 + x3)<<oss;
tmp = (tmp*4 + x3)<<OVERSAMPLING;
b3 = (tmp+2)/4;
x1 = ac3 * b6 >> 13;
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING);
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8);

View File

@ -28,14 +28,11 @@ class AP_Baro_BMP085 : public AP_Baro
private:
int32_t RawPress;
int32_t _offset_press;
int32_t _offset_press;
int32_t RawTemp;
int16_t Temp;
int32_t Press;
//int Altitude;
uint8_t oss;
uint32_t Press;
bool _apm2_hardware;
//int32_t Press0; // Pressure at sea level
// State machine
@ -44,9 +41,9 @@ class AP_Baro_BMP085 : public AP_Baro
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
int _temp_filter[TEMP_FILTER_SIZE];
int _press_filter[PRESS_FILTER_SIZE];
long _offset_temp;
int16_t _temp_filter[TEMP_FILTER_SIZE];
int32_t _press_filter[PRESS_FILTER_SIZE];
int32_t _offset_temp;
uint8_t _temp_index;
uint8_t _press_index;

View File

@ -17,7 +17,6 @@ public:
//int Altitude;
bool healthy;
uint8_t oss;
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
@ -26,7 +25,6 @@ public:
int32_t get_raw_pressure();
int32_t get_raw_temp();
void setHIL(float Temp, float Press);
int32_t _offset_press;
};
#endif // __AP_BARO_BMP085_HIL_H__