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

SITL: SIM_MS5611 fix D1&D2 when temperature compensated & forward_conversion check

This commit is contained in:
Josh Henderson 2021-08-15 02:30:21 -04:00 committed by Peter Barker
parent cb0b12f25c
commit aaa8600d7b
2 changed files with 44 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/SITL/SIM_MS5611.cpp
View File

@ -9,12 +9,16 @@ using namespace SITL;
// forward conversion, copied from driver:
void MS5611::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
{
// _D1 and _D2 are stored as floats in driver
const float _D1 = D1;
const float _D2 = D2;
float dT;
float TEMP;
float OFF;
float SENS;
dT = D2-(((uint32_t)prom[5])<<8);
dT = _D2-(((uint32_t)prom[5])<<8);
TEMP = (dT * prom[6])/8388608;
OFF = prom[2] * 65536.0f + (prom[4] * dT) / 128;
SENS = prom[1] * 32768.0f + (prom[3] * dT) / 256;
@ -37,43 +41,62 @@ void MS5611::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
SENS = SENS - SENS2;
}
P_Pa = (D1*SENS/2097152 - OFF)/32768;
P_Pa = (_D1*SENS/2097152 - OFF)/32768;
Temp_C = TEMP * 0.01f;
}
void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
{
int64_t TEMP = Temp_C * 100.0f;
const float dT = (TEMP-2000.0) / (prom[6]/8388608.0);
float OFF = prom[2] * 65536.0f + (prom[4] * dT) / 128;
float SENS = prom[1] * 32768.0f + (prom[3] * dT) / 256;
const uint8_t Q1 = Qx_coeff[0];
const uint8_t Q2 = Qx_coeff[1];
const uint8_t Q3 = Qx_coeff[2];
const uint8_t Q4 = Qx_coeff[3];
const uint8_t Q5 = Qx_coeff[4];
const uint8_t Q6 = Qx_coeff[5];
const float TEMP = Temp_C * 100;
// second order temperature compensation when under 20 degrees C
if (TEMP < 2000) {
// second order temperature compensation when under 20 degrees C
float T2 = (dT*dT) / 0x80000000;
float Aux = sq(TEMP-2000.0);
float OFF2 = 2.5f*Aux;
float SENS2 = 1.25f*Aux;
// Solve the quadratic equation for D2 when TEMP < 2000
D2 = 128 * (2 * prom[5] - sqrt(sq(prom[6]) - 131072 * (TEMP - 2000)) + prom[6]);
// Must compute the pressure compensation values using D2
const float dT = float(D2) - (prom[5] << Q5);
float TEMP_forward = 2000 + (dT * prom[6]) / (1L << Q6);
float OFF = prom[2] * (1L << Q2) + (prom[4] * dT) / (1L << Q4);
float SENS = prom[1] * (1L << Q1) + (prom[3] * dT) / (1L << Q3);
const float Aux = sq(TEMP_forward - 2000);
float OFF2 = 2.5 * Aux;
float SENS2 = 1.25 * Aux;
if (TEMP < -1500) {
// extra compensation for temperatures below -15C
OFF2 += 7 * sq(TEMP+1500);
SENS2 += sq(TEMP+1500) * 11.0*0.5;
OFF2 += 7 * sq(TEMP_forward + 1500);
SENS2 += sq(TEMP_forward + 1500) * 11.0 * 0.5;
}
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
D1 = ((P_Pa*32768.0)+OFF) / (SENS/2097152.0);
D2 = dT + (((uint32_t)prom[5])<<8);
D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
} else {
const float dT = (TEMP - 2000) * (1L << Q6) / prom[6];
const float OFF = prom[2] * (1L << Q2) + (prom[4] * dT) / (1L << Q4);
const float SENS = prom[1] * (1L << Q1) + (prom[3] * dT) / (1L << Q3);
D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
D2 = dT + (prom[5] << Q5);
}
float f_P_Pa;
float f_Temp_C;
convert_forward(D1, D2, f_P_Pa, f_Temp_C);
if (fabs(f_P_Pa - P_Pa) > 1) {
if (fabs(f_P_Pa - P_Pa) > 0.2) {
AP_HAL::panic("Invalid pressure conversion");
}
if (fabs(f_Temp_C - Temp_C) > 0.1) {
if (fabs(f_Temp_C - Temp_C) > 0.02) {
AP_HAL::panic("Invalid temperature conversion");
}
}

2
libraries/SITL/SIM_MS5611.h
View File

@ -36,7 +36,7 @@ private:
};
const uint8_t Qx_coeff[6] {
15, 17, 7, 5, 7, 21
15, 16, 8, 7, 8, 23
};
};