aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: Add support to MS5637

Browse Source 
As AVR2560 is not supported anymore and do integer operations is
usually faster than float-point the _calculate() implementation was
done using only integer operations and as more close to what
datasheet says.
This commit is contained in:
José Roberto de Souza 2015-09-28 15:31:12 -03:00 committed by Andrew Tridgell
parent 6bdeae97b6
commit d9931b5f34
2 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -449,6 +449,45 @@ void AP_Baro_MS5607::_calculate()
_copy_to_frontend(_instance, pressure, temperature);
}
/* MS563 Class */
AP_Baro_MS5637::AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer)
: AP_Baro_MS56XX(baro, serial, use_timer)
{
}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5637::_calculate()
{
int32_t dT, TEMP;
int64_t OFF, SENS;
int32_t raw_pressure = _D1;
int32_t raw_temperature = _D2;
// Formulas from manufacturer datasheet
// sub -15c temperature compensation is not included
dT = raw_temperature - (((uint32_t)_C5) << 8);
TEMP = 2000 + ((int64_t)dT * (int64_t)_C6) / 8388608;
OFF = (int64_t)_C2 * (int64_t)131072 + ((int64_t)_C4 * (int64_t)dT) / (int64_t)64;
SENS = (int64_t)_C1 * (int64_t)65536 + ((int64_t)_C3 * (int64_t)dT) / (int64_t)128;
if (TEMP < 2000) {
// second order temperature compensation when under 20 degrees C
int32_t T2 = ((int64_t)3 * ((int64_t)dT * (int64_t)dT) / (int64_t)8589934592);
int64_t aux = (TEMP - 2000) * (TEMP - 2000);
int64_t OFF2 = 61 * aux / 16;
int64_t SENS2 = 29 * aux / 16;
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)32768;
float temperature = TEMP * 0.01f;
_copy_to_frontend(_instance, (float)pressure, temperature);
}
/*
Read the sensor from main code. This is only used for I2C MS5611 to
avoid conflicts on the semaphore from calling it in a timer, which

8
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -119,4 +119,12 @@ public:
private:
void _calculate();
};
class AP_Baro_MS5637 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
private:
void _calculate();
};
#endif // __AP_BARO_MS5611_H__