From 18a063227623bcf96b38c9228fea79dbb9c81b1a Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Tue, 17 Aug 2021 03:43:26 -0400 Subject: [PATCH] SITL: MS5XXX drivers use check_conversion_accuracy --- libraries/SITL/SIM_MS5525.cpp | 17 ++++++++++++----- libraries/SITL/SIM_MS5525.h | 1 + libraries/SITL/SIM_MS5611.cpp | 4 +++- libraries/SITL/SIM_MS5611.h | 1 + libraries/SITL/SIM_MS5XXX.cpp | 4 ++++ libraries/SITL/SIM_MS5XXX.h | 2 +- 6 files changed, 22 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp index ab7b7675c3..c0178577e0 100644 --- a/libraries/SITL/SIM_MS5525.cpp +++ b/libraries/SITL/SIM_MS5525.cpp @@ -44,15 +44,22 @@ void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) D2 = dT + int64_t(prom[5]) * (1L< 0.1) { - AP_HAL::panic("Invalid pressure conversion"); + + const float p_error = fabs(f_P_Pa - P_Pa); + const float p_error_max = 0.1; + if (p_error > p_error_max) { + AP_HAL::panic("Invalid pressure conversion: error %f Pa > %f Pa error max", p_error, p_error_max); } - if (fabs(f_Temp_C - Temp_C) > 0.01) { - AP_HAL::panic("Invalid temperature conversion"); + + const float t_error = fabs(f_Temp_C - Temp_C); + const float t_error_max = 0.01; + if (t_error > t_error_max) { + AP_HAL::panic("Invalid temperature conversion: error %f degC > %f degC error max", t_error, t_error_max); } } diff --git a/libraries/SITL/SIM_MS5525.h b/libraries/SITL/SIM_MS5525.h index 652ed2140e..f64b82ff0b 100644 --- a/libraries/SITL/SIM_MS5525.h +++ b/libraries/SITL/SIM_MS5525.h @@ -14,6 +14,7 @@ protected: void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) override; void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) override; + void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) override; void load_prom(uint16_t *_loaded_prom, uint8_t len) const override { memcpy(_loaded_prom, prom, len); diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp index 1ea83cf11c..bc7f91a6f7 100644 --- a/libraries/SITL/SIM_MS5611.cpp +++ b/libraries/SITL/SIM_MS5611.cpp @@ -88,7 +88,10 @@ 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; D2 = dT + (prom[5] << Q5); } +} +void MS5611::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) +{ float f_P_Pa; float f_Temp_C; convert_forward(D1, D2, f_P_Pa, f_Temp_C); @@ -101,7 +104,6 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) } } - void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C) { float sigma, delta, theta; diff --git a/libraries/SITL/SIM_MS5611.h b/libraries/SITL/SIM_MS5611.h index bd5235f9ea..7822a470d4 100644 --- a/libraries/SITL/SIM_MS5611.h +++ b/libraries/SITL/SIM_MS5611.h @@ -16,6 +16,7 @@ protected: void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) override; void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) override; + void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) override; void load_prom(uint16_t *_loaded_prom, uint8_t len) const override { memcpy(_loaded_prom, prom, len); diff --git a/libraries/SITL/SIM_MS5XXX.cpp b/libraries/SITL/SIM_MS5XXX.cpp index 516b876204..88c1e6f76e 100644 --- a/libraries/SITL/SIM_MS5XXX.cpp +++ b/libraries/SITL/SIM_MS5XXX.cpp @@ -34,6 +34,10 @@ void MS5XXX::convert_D1() uint32_t D1; uint32_t D2; convert(P_Pa, temperature, D1, D2); + + // Check the accuracy of the returned conversion by utilizing a copy of the drivers + check_conversion_accuracy(P_Pa, temperature, D1, D2); + convert_out[2] = D1 & 0xff; D1 >>= 8; convert_out[1] = D1 & 0xff; diff --git a/libraries/SITL/SIM_MS5XXX.h b/libraries/SITL/SIM_MS5XXX.h index e10a126aec..345722b2ba 100644 --- a/libraries/SITL/SIM_MS5XXX.h +++ b/libraries/SITL/SIM_MS5XXX.h @@ -82,7 +82,7 @@ private: virtual void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) =0; virtual void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) = 0; virtual void get_pressure_temperature_readings(float &P_Pa, float &Temp_C) = 0; - + virtual void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) = 0; void convert_D1(); void convert_D2();