rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +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 95b12c360f
commit 7c6f585b80
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 #ifdef PIOS_INCLUDE_MS5611
#define POW2(x) (2 << (x - 1))
// TODO: Clean this up. Getting around old constant. // TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling #define PIOS_MS5611_OVERSAMPLING oversampling
@ -189,9 +190,12 @@ int32_t PIOS_MS5611_ReadADC(void)
} }
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
// Difference between actual and reference temperature
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8); // dT = D2 - TREF = D2 - C5 * 2^8
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23); 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 { } else {
int64_t Offset; int64_t Offset;
int64_t Sens; int64_t Sens;
@ -205,15 +209,22 @@ int32_t PIOS_MS5611_ReadADC(void)
} }
// check if temperature is less than 20°C // check if temperature is less than 20°C
if (Temperature < 2000) { 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); int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
Offset2 = (5 * tcorr) >> 1; Offset2 = (5 * tcorr) / 2;
Sens2 = (5 * tcorr) >> 2; Sens2 = (5 * tcorr) / 4;
compensation_t2 = (deltaTemp * deltaTemp) >> 31; compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C // Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) { if (Temperature < -1500) {
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500); int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr2; Offset2 += 7 * tcorr2;
Sens2 += (11 * tcorr2) >> 1; Sens2 += (11 * tcorr2) / 2;
} }
} else { } else {
compensation_t2 = 0; compensation_t2 = 0;
@ -221,10 +232,15 @@ int32_t PIOS_MS5611_ReadADC(void)
Sens2 = 0; Sens2 = 0;
} }
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; // Offset at actual temperature
Sens = ((int64_t)CalibData.C[0]) << 15; // OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2; Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; // 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; return 0;
} }