rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-1139: perform second order low temperature compensation.

Browse Source 
It follows the procedure described in MS5611 datasheet(http://www.meas-spec.com/downloads/MS5611-01BA03.pdf, page 8)
to perform low(20°) and very low(-15°C) temperature compensation.
This commit is contained in:
Alessio Morale 2013-12-10 01:21:05 +01:00
parent 8a742179a7
commit 75842cb648
1 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
flight/pios/common/pios_ms5611.c
View File

@ -51,7 +51,7 @@ static int64_t Temperature;
static int32_t lastConversionStart;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
static int64_t t2;
// Move into proper driver structure with cfg stored
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
@ -74,6 +74,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
uint8_t data[2];
// reset calibration values
t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@ -190,17 +193,34 @@ int32_t PIOS_MS5611_ReadADC(void)
} else {
int64_t Offset;
int64_t Sens;
// used for second order temperature compensation
int64_t Offset2 = 0;
int64_t Sens2 = 0;
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
}
// check if temperature is less than 20°C
if(Temperature < 2000){
Offset2 = 5 * (Temperature - 2000) >> 1;
Sens2 = Offset2 >> 1;
t2 = (deltaTemp*deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if(Temperature < -1500){
int64_t tcorr = (Temperature + 1500)*(Temperature + 1500);
Offset2 += 7 * tcorr;
Sens2 += (11 * tcorr) >> 1;
}
} else {
t2 = 0;
Offset2 = 0;
Sens2 = 0;
}
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2;
Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8);
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
}
return 0;
@ -211,7 +231,7 @@ int32_t PIOS_MS5611_ReadADC(void)
*/
float PIOS_MS5611_GetTemperature(void)
{
return ((float)Temperature) / 100.0f;
return ((float)(Temperature - t2)) / 100.0f;
}
/**