aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: MS5XXX drivers use check_conversion_accuracy

Browse Source
This commit is contained in:
Josh Henderson 2021-08-17 03:43:26 -04:00 committed by Andrew Tridgell
parent 2a2399c8f1
commit 18a0632276
6 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_MS5525.cpp
View File

@ -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<<Q5);
}
void MS5525::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);
if (fabs(f_P_Pa - P_Pa) > 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);
}
}

1
libraries/SITL/SIM_MS5525.h
View File

@ -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);

4
libraries/SITL/SIM_MS5611.cpp
View File

@ -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;

1
libraries/SITL/SIM_MS5611.h
View File

@ -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);

4
libraries/SITL/SIM_MS5XXX.cpp
View File

@ -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;

2
libraries/SITL/SIM_MS5XXX.h
View File

@ -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();