diff --git a/libraries/SITL/SIM_MS5XXX.cpp b/libraries/SITL/SIM_MS5XXX.cpp index 39cdfd3ec0..516b876204 100644 --- a/libraries/SITL/SIM_MS5XXX.cpp +++ b/libraries/SITL/SIM_MS5XXX.cpp @@ -1,4 +1,4 @@ -#include "SIM_MS5525.h" +#include "SIM_MS5XXX.h" #include @@ -6,101 +6,56 @@ using namespace SITL; -MS5525::MS5525() : +MS5XXX::MS5XXX() : I2CDevice() { } -void MS5525::reset() +void MS5XXX::reset() { // load prom from internal register: prom_loaded = true; + load_prom(loaded_prom, sizeof(loaded_prom)); } -void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) +void MS5XXX::convert_D1() { - 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]; + float P_Pa; + float temperature; + get_pressure_temperature_readings(P_Pa, temperature); - // this is the forward conversion copied from the driver: - int64_t dT = D2 - int64_t(prom[5]) * (1UL< 1) { - AP_HAL::panic("Invalid conversion"); - } - if (fabs(f_Temp_C - Temp_C) > 0.1) { - AP_HAL::panic("Invalid conversion"); - } -} - -void MS5525::convert_D1() -{ - float pressure = AP::sitl()->state.airspeed_raw_pressure[0]; - if (pressure < 0.1) { + if (P_Pa < 0.1) { // maths breaks down on very, very low numbers, or there's a // bug in the conversion code. The simulation can pass in // very, very low numbers. Clamp it. - pressure = 0.1; + P_Pa = 0.1; } - const float temperature = 25.0f; uint32_t D1; uint32_t D2; - convert(pressure, temperature, D1, D2); + convert(P_Pa, temperature, D1, D2); convert_out[2] = D1 & 0xff; D1 >>= 8; convert_out[1] = D1 & 0xff; D1 >>= 8; convert_out[0] = D1 & 0xff; } -void MS5525::convert_D2() +void MS5XXX::convert_D2() { - float pressure = AP::sitl()->state.airspeed_raw_pressure[0]; - if (pressure < 0.1) { + float P_Pa; + float temperature; + get_pressure_temperature_readings(P_Pa, temperature); + + if (P_Pa < 0.1) { // maths breaks down on very, very low numbers, or there's a // bug in the conversion code. The simulation can pass in // very, very low numbers. Clamp it. - pressure = 0.1; + P_Pa = 0.1; } - const float temperature = 25.0f; uint32_t D1; uint32_t D2; - convert(pressure, temperature, D1, D2); + convert(P_Pa, temperature, D1, D2); convert_out[2] = D2 & 0xff; D2 >>= 8; convert_out[1] = D2 & 0xff; @@ -109,7 +64,7 @@ void MS5525::convert_D2() } -void MS5525::update(const class Aircraft &aircraft) +void MS5XXX::update(const class Aircraft &aircraft) { const uint32_t now_us = AP_HAL::micros(); // static uint32_t then_us = 0; @@ -184,7 +139,7 @@ void MS5525::update(const class Aircraft &aircraft) // float temperature = 25.0f; } -int MS5525::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +int MS5XXX::rdwr(I2C::i2c_rdwr_ioctl_data *&data) { struct I2C::i2c_msg &msg = data->msgs[0]; // if (data->nmsgs != 1) { @@ -225,7 +180,7 @@ int MS5525::rdwr(I2C::i2c_rdwr_ioctl_data *&data) AP_HAL::panic("Unexpected length"); } const uint8_t addr = ((unsigned)cmd - (unsigned)Command::READ_C0)/2; - const uint16_t val = htobe16(prom[addr]); + const uint16_t val = htobe16(loaded_prom[addr]); data->msgs[1].buf[0] = val & 0xff; data->msgs[1].buf[1] = val >> 8; break; diff --git a/libraries/SITL/SIM_MS5XXX.h b/libraries/SITL/SIM_MS5XXX.h index dd7ca717bc..e10a126aec 100644 --- a/libraries/SITL/SIM_MS5XXX.h +++ b/libraries/SITL/SIM_MS5XXX.h @@ -1,14 +1,16 @@ +#pragma once + #include "SIM_I2CDevice.h" #include namespace SITL { -class MS5525 : public I2CDevice +class MS5XXX : public I2CDevice { public: - MS5525(); + MS5XXX(); protected: @@ -69,37 +71,21 @@ private: uint32_t command_start_us; - uint8_t read_prom_addr; - - uint8_t convert_out[3]; - // this data comes from the datasheet page 7 - const uint16_t prom[8] { - 0xFFFF, // reserved - 36402, // C1, pressure sensitivity - 39473, // C2, pressure offset - 40393, // C3, temperature coeff of press sensit - 29523, // C4, temperature cofff of press offs - 29854, // C5, ref temperature - 21917, // C6, temperature coeff of temperature - 0x000c // checksum - }; bool prom_loaded = false; - - // for 5525DSO-pp001DS - const uint8_t Qx_coeff[6] { - 15, 17, 7, 5, 7, 21 - }; - + uint16_t loaded_prom[128/16]; + virtual void load_prom(uint16_t *loaded_prom, uint8_t len) const = 0; uint16_t conversion_time_osr_1024_us = 2280; - void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2); + 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; + + void convert_D1(); void convert_D2(); - - void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C); }; } // namespace SITL