rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1149 Remove shifts as they can have bad behaviours for negative numbers.

Browse Source 
Documents correction algorithms as described in ms5611 documentation
This commit is contained in:
Alessio Morale 2014-02-08 15:29:14 +01:00
parent 5fa390ec9d
commit 1569881e94
1 changed files with 26 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,6 +32,7 @@
#ifdef PIOS_INCLUDE_MS5611
#define POW2(x) (2 << (x - 1))
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
@ -189,9 +190,12 @@ int32_t PIOS_MS5611_ReadADC(void)
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8);
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23);
// Difference between actual and reference temperature
// dT = D2 - TREF = D2 - C5 * 2^8
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8));
// Actual temperature (-40…85°C with 0.01°C resolution)
// TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
} else {
int64_t Offset;
int64_t Sens;
@ -205,15 +209,22 @@ int32_t PIOS_MS5611_ReadADC(void)
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
// Apply compensation
// T2 = dT^2 / 2^31
// OFF2 = 5 ⋅ (TEMP – 2000)^2/2
// SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
Offset2 = (5 * tcorr) >> 1;
Sens2 = (5 * tcorr) >> 2;
Offset2 = (5 * tcorr) / 2;
Sens2 = (5 * tcorr) / 4;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) {
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr2;
Sens2 += (11 * tcorr2) >> 1;
Sens2 += (11 * tcorr2) / 2;
}
} else {
compensation_t2 = 0;
@ -221,10 +232,15 @@ int32_t PIOS_MS5611_ReadADC(void)
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) - Offset2;
Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
// Offset at actual temperature
// OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7
Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2;
// Sensitivity at actual temperature
// SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8
Sens = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2;
// Temperature compensated pressure (10…1200mbar with 0.01mbar resolution)
// P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
}
return 0;
}