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

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 bcd63a9807
commit 897d9a1c0b
3 changed files with 15 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

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

13
libraries/AP_Baro/AP_Baro_BMP085.h
View File

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

2
libraries/AP_Baro/AP_Baro_BMP085_hil.h
View File

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