zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: adjust MS5XXX to be new base class

gps-1.3.1
Peter Barker 4 years ago committed by Andrew Tridgell
parent
50d7fc353f
commit
aa97c5b714
2 changed files with 32 additions and 91 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 89
      libraries/SITL/SIM_MS5XXX.cpp
  2. 34
      libraries/SITL/SIM_MS5XXX.h

89
libraries/SITL/SIM_MS5XXX.cpp
Unescape View File

@ -1,4 +1,4 @@ @@ -1,4 +1,4 @@
#include "SIM_MS5525.h"
#include "SIM_MS5XXX.h"
#include <SITL/SITL.h>
@ -6,101 +6,56 @@ @@ -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<<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);
const float PSI_to_Pa = 6894.757f;
P_Pa = PSI_to_Pa * 1.0e-4 * P;
Temp_C = TEMP * 0.01;
}
void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
{
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];
const int64_t TEMP = Temp_C * 100.0f;
const float dT = ((TEMP-2000)*(1UL<<Q6))/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);
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_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() @@ -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) @@ -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) @@ -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;

34
libraries/SITL/SIM_MS5XXX.h
Unescape View File

@ -1,14 +1,16 @@ @@ -1,14 +1,16 @@
#pragma once
#include "SIM_I2CDevice.h"
#include <AP_Common/Bitmask.h>
namespace SITL {
class MS5525 : public I2CDevice
class MS5XXX : public I2CDevice
{
public:
MS5525();
MS5XXX();
protected:
@ -69,37 +71,21 @@ private: @@ -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;
uint16_t loaded_prom[128/16];
virtual void load_prom(uint16_t *loaded_prom, uint8_t len) const = 0;
// for 5525DSO-pp001DS
const uint8_t Qx_coeff[6] {
15, 17, 7, 5, 7, 21
};
uint16_t conversion_time_osr_1024_us = 2280;
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;
uint16_t conversion_time_osr_1024_us = 2280;
void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2);
void convert_D1();
void convert_D2();
void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C);
};
} // namespace SITL

Write Preview
Loading…
Cancel
Save