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

SITL: sim_MS5611 fix simulated driver for 32bit usage

Browse Source
This commit is contained in:
Josh Henderson 2021-09-07 01:40:48 -04:00 committed by Andrew Tridgell
parent 27bb200644
commit 4d7dddf337
2 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/SITL/SIM_MS5611.cpp
View File

@ -59,13 +59,13 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
// second order temperature compensation when under 20 degrees C
if (TEMP < 2000) {
// Solve the quadratic equation for D2 when TEMP < 2000
D2 = 128 * (2 * prom[5] - sqrt(sq(prom[6]) - 131072 * (TEMP - 2000)) + prom[6]);
D2 = 128 * (2 * int64_t(prom[5]) - sqrt(sq(int64_t(prom[6])) - 131072 * (TEMP - 2000)) + int64_t(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 dT = float(D2) - (int64_t(prom[5]) << Q5);
float TEMP_forward = 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);
const float Aux = sq(TEMP_forward - 2000);
float OFF2 = 2.5 * Aux;
@ -81,12 +81,12 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
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);
const float dT = (TEMP - 2000) * (1L << Q6) / int64_t(prom[6]);
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_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
D2 = dT + (prom[5] << Q5);
D2 = dT + (int64_t(prom[5]) << Q5);
}
}

6
libraries/SITL/tests/test_sim_ms5611.cpp
View File

@ -63,8 +63,10 @@ TEST(MS5611, convert)
uint32_t D1;
uint32_t D2;
ms5611.convert_pub(elem.P_Pa, elem.Temp_C, D1, D2);
EXPECT_EQ(D1, elem.D1);
EXPECT_EQ(D2, elem.D2);
// Expect NEAR here instead of EQ because in 32bit they are off by 1
EXPECT_NEAR(D1, elem.D1, 1);
EXPECT_NEAR(D2, elem.D2, 1);
}
}