Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fix MS5525 for temp <20C and negative pressures

Browse Source
This commit is contained in:
Josh Henderson 2021-08-16 03:14:21 -04:00 committed by Andrew Tridgell
parent aa402d9a55
commit 5e90649a6e
1 changed files with 25 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/SITL/SIM_MS5525.cpp
View File

@ -14,11 +14,11 @@ void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
const uint8_t Q6 = Qx_coeff[5];
// this is the forward conversion copied from the driver:
int64_t dT = D2 - int64_t(prom[5]) * (1UL<<Q5);
int64_t TEMP = 2000 + (dT*int64_t(prom[6]))/(1UL<<Q6);
int64_t OFF = int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
int64_t P = (D1*SENS/(1UL<<21)-OFF)/(1UL<<15);
float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
float OFF = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
const float PSI_to_Pa = 6894.757f;
P_Pa = PSI_to_Pa * 1.0e-4 * P;
Temp_C = TEMP * 0.01;
@ -33,29 +33,38 @@ void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
const uint8_t Q5 = Qx_coeff[4];
const uint8_t Q6 = Qx_coeff[5];
const int64_t TEMP = Temp_C * 100.0f;
const float dT = ((TEMP-2000)*(1UL<<Q6))/prom[6];
const float TEMP = Temp_C * 100.0f;
const float dT = ((TEMP - 2000.0) * (1L<<Q6)) / int64_t(prom[6]);
const float PSI_to_Pa = 6894.757f;
const float P = P_Pa / (PSI_to_Pa * 1.0e-4);
const int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
const int64_t OFF = int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
D1 = (((uint64_t(P*(1U<<15)))+OFF)<<21)/SENS;
D2 = dT + int64_t(prom[5]) * (1UL<<Q5);
const float OFF = int64_t(prom[2]) * (1L<<Q2) + (int64_t(prom[4]) * dT) / (1L<<Q4);
const float SENS = int64_t(prom[1]) * (1L<<Q1) + (int64_t(prom[3]) * dT) / (1L<<Q3);
D1 = ((P * (1L<<15) + OFF) * (1L<<21)) / SENS;
D2 = dT + int64_t(prom[5]) * (1L<<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) {
AP_HAL::panic("Invalid conversion");
if (fabs(f_P_Pa - P_Pa) > 0.1) {
AP_HAL::panic("Invalid pressure conversion");
}
if (fabs(f_Temp_C - Temp_C) > 0.1) {
AP_HAL::panic("Invalid conversion");
if (fabs(f_Temp_C - Temp_C) > 0.01) {
AP_HAL::panic("Invalid temperature conversion");
}
}
void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
{
Temp_C = 25.0f;
float sim_alt = AP::sitl()->state.altitude;
sim_alt += 2 * rand_float();
float sigma, delta, theta;
AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
// To Do: Add a sensor board temperature offset parameter
Temp_C = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + 25.0;
P_Pa = AP::sitl()->state.airspeed_raw_pressure[0];
}