mirror of https://github.com/ArduPilot/ardupilot
SITL: add support for ms5525 i2c sensor
This commit is contained in:
Peter Barker
Peter Barker
parent
e3b366419b
commit
7375d1949c
|
|
@ -28,6 +28,7 @@
|
|||
#include "SIM_Airspeed_DLVR.h"
|
||||
#include "SIM_Temperature_TSYS01.h"
|
||||
#include "SIM_ICM40609.h"
|
||||
#include "SIM_MS5525.h"
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
|
|
@ -54,6 +55,7 @@ static Rotoye rotoye;
|
|||
static Airspeed_DLVR airspeed_dlvr;
|
||||
static TSYS01 tsys01;
|
||||
static ICM40609 icm40609;
|
||||
static MS5525 ms5525;
|
||||
|
||||
struct i2c_device_at_address {
|
||||
uint8_t bus;
|
||||
|
|
@ -67,7 +69,7 @@ struct i2c_device_at_address {
|
|||
{ 1, 0x38, ignored }, // NCP5623
|
||||
{ 1, 0x39, ignored }, // NCP5623C
|
||||
{ 1, 0x40, ignored }, // KellerLD
|
||||
{ 1, 0x76, ignored }, // MS56XX
|
||||
{ 1, 0x76, ms5525 },
|
||||
{ 1, 0x77, tsys01 },
|
||||
{ 1, 0x0B, rotoye },
|
||||
{ 2, 0x0B, maxell },
|
||||
|
|
|
|||
|
|
@ -0,0 +1,257 @@
|
|||
#include "SIM_MS5525.h"
|
||||
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
MS5525::MS5525() :
|
||||
I2CDevice()
|
||||
{
|
||||
}
|
||||
|
||||
void MS5525::reset()
|
||||
{
|
||||
// load prom from internal register:
|
||||
prom_loaded = true;
|
||||
}
|
||||
|
||||
void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
|
||||
{
|
||||
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];
|
||||
|
||||
// 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) {
|
||||
// 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;
|
||||
}
|
||||
const float temperature = 25.0f;
|
||||
|
||||
uint32_t D1;
|
||||
uint32_t D2;
|
||||
convert(pressure, 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()
|
||||
{
|
||||
float pressure = AP::sitl()->state.airspeed_raw_pressure[0];
|
||||
if (pressure < 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;
|
||||
}
|
||||
const float temperature = 25.0f;
|
||||
|
||||
uint32_t D1;
|
||||
uint32_t D2;
|
||||
convert(pressure, temperature, D1, D2);
|
||||
convert_out[2] = D2 & 0xff;
|
||||
D2 >>= 8;
|
||||
convert_out[1] = D2 & 0xff;
|
||||
D2 >>= 8;
|
||||
convert_out[0] = D2 & 0xff;
|
||||
}
|
||||
|
||||
|
||||
void MS5525::update(const class Aircraft &aircraft)
|
||||
{
|
||||
const uint32_t now_us = AP_HAL::micros();
|
||||
// static uint32_t then_us = 0;
|
||||
// ::fprintf(stderr, "update: s=%u now=%u delta=%u cmd-age=%u\n", (unsigned)state, (unsigned)now_us, (unsigned)(now_us - then_us), (unsigned)(now_us-command_start_us));
|
||||
// then_us = now_us;
|
||||
|
||||
switch (state) {
|
||||
case State::COLD:
|
||||
command_start_us = now_us;
|
||||
prom_loaded = false;
|
||||
state = State::COLD_WAIT;
|
||||
break;
|
||||
case State::COLD_WAIT:
|
||||
// 1ms to do anything....
|
||||
if (now_us - command_start_us < 1) {
|
||||
break;
|
||||
}
|
||||
state = State::UNINITIALISED;
|
||||
FALLTHROUGH;
|
||||
case State::UNINITIALISED:
|
||||
break;
|
||||
case State::RESET_START:
|
||||
command_start_us = now_us;
|
||||
state = State::RESET_WAIT;
|
||||
break;
|
||||
case State::RESET_WAIT:
|
||||
// 2ms for reset to complete (data sheet does not specify?)
|
||||
if (now_us - command_start_us > 2000) {
|
||||
reset();
|
||||
state = State::RUNNING;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::CONVERSION_D1_START:
|
||||
command_start_us = now_us;
|
||||
convert_out[0] = 0;
|
||||
convert_out[1] = 0;
|
||||
convert_out[2] = 0;
|
||||
state = State::CONVERSION_D1_WAIT;
|
||||
break;
|
||||
case State::CONVERSION_D1_WAIT:
|
||||
// driver allows for 10ms for a conversion to happen
|
||||
if (now_us - command_start_us > conversion_time_osr_1024_us) {
|
||||
convert_D1();
|
||||
state = State::RUNNING;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::CONVERSION_D2_START:
|
||||
command_start_us = now_us;
|
||||
convert_out[0] = 0;
|
||||
convert_out[1] = 0;
|
||||
convert_out[2] = 0;
|
||||
state = State::CONVERSION_D2_WAIT;
|
||||
break;
|
||||
case State::CONVERSION_D2_WAIT:
|
||||
// driver allows for 10ms for a conversion to happen
|
||||
if (now_us - command_start_us > conversion_time_osr_1024_us) {
|
||||
convert_D2();
|
||||
state = State::RUNNING;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::RUNNING:
|
||||
break;
|
||||
}
|
||||
|
||||
// float pressure = AP::sitl()->state.airspeed_raw_pressure[0];
|
||||
// float temperature = 25.0f;
|
||||
}
|
||||
|
||||
int MS5525::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
||||
{
|
||||
struct I2C::i2c_msg &msg = data->msgs[0];
|
||||
// if (data->nmsgs != 1) {
|
||||
// AP_HAL::panic("nmsgs=%u", data->nmsgs);
|
||||
// }
|
||||
if (msg.flags == I2C_M_RD) {
|
||||
AP_HAL::panic("Read (%u)",msg.len);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (msg.len != 1) {
|
||||
AP_HAL::panic("bad command length");
|
||||
}
|
||||
const Command cmd = (Command)msg.buf[0];
|
||||
if (state != State::RUNNING) {
|
||||
if (state == State::UNINITIALISED &&
|
||||
cmd == Command::RESET) {
|
||||
// this is OK - RESET is OK in UNINITIALISED
|
||||
} else {
|
||||
::fprintf(stderr, "Command (0x%02x) received while not running (state=%u)\n", (unsigned)cmd, (unsigned)state);
|
||||
return -1; // we could die instead...
|
||||
}
|
||||
}
|
||||
|
||||
switch (cmd) {
|
||||
case Command::RESET:
|
||||
state = State::RESET_START;
|
||||
break;
|
||||
case Command::READ_C0:
|
||||
case Command::READ_C1:
|
||||
case Command::READ_C2:
|
||||
case Command::READ_C3:
|
||||
case Command::READ_C4:
|
||||
case Command::READ_C5:
|
||||
case Command::READ_C6:
|
||||
case Command::READ_CRC: {
|
||||
if (data->msgs[1].len != 2) {
|
||||
AP_HAL::panic("Unexpected length");
|
||||
}
|
||||
const uint8_t addr = ((unsigned)cmd - (unsigned)Command::READ_C0)/2;
|
||||
const uint16_t val = htobe16(prom[addr]);
|
||||
data->msgs[1].buf[0] = val & 0xff;
|
||||
data->msgs[1].buf[1] = val >> 8;
|
||||
break;
|
||||
}
|
||||
case Command::CONVERT_D1_OSR_1024:
|
||||
state = State::CONVERSION_D1_START;
|
||||
break;
|
||||
case Command::CONVERT_D2_OSR_1024:
|
||||
state = State::CONVERSION_D2_START;
|
||||
break;
|
||||
case Command::READ_CONVERSION:
|
||||
if (data->msgs[1].len == 0) {
|
||||
// upon not getting a reading back the driver commands a
|
||||
// conversion-read but doesn't wait for a response!
|
||||
::fprintf(stderr, "read of length zero\n");
|
||||
return -1;
|
||||
}
|
||||
if (data->msgs[1].len != 3) {
|
||||
AP_HAL::panic("Unexpected length=%u", data->msgs[1].len);
|
||||
}
|
||||
data->msgs[1].buf[0] = convert_out[0];
|
||||
data->msgs[1].buf[1] = convert_out[1];
|
||||
data->msgs[1].buf[2] = convert_out[2];
|
||||
break;
|
||||
default:
|
||||
AP_HAL::panic("Unknown command %u (0x%02x)", (unsigned)cmd, (unsigned)cmd);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
#include "SIM_I2CDevice.h"
|
||||
|
||||
#include <AP_Common/Bitmask.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class MS5525 : public I2CDevice
|
||||
{
|
||||
public:
|
||||
|
||||
MS5525();
|
||||
|
||||
protected:
|
||||
|
||||
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
|
||||
|
||||
void update(const class Aircraft &aircraft) override;
|
||||
|
||||
private:
|
||||
|
||||
// float pressure;
|
||||
// float temperature;
|
||||
|
||||
// time we last updated the measurements (simulated internal
|
||||
// workings of sensor)
|
||||
uint32_t last_update_ms;
|
||||
|
||||
void reset();
|
||||
|
||||
enum class Command : uint8_t {
|
||||
RESET = 0x1E,
|
||||
|
||||
READ_CONVERSION = 0x00,
|
||||
|
||||
// prom reading commands:
|
||||
READ_C0 = 0xa0,
|
||||
READ_C1 = 0xa2,
|
||||
READ_C2 = 0xa4,
|
||||
READ_C3 = 0xa6,
|
||||
READ_C4 = 0xa8,
|
||||
READ_C5 = 0xaa,
|
||||
READ_C6 = 0xac,
|
||||
READ_CRC = 0xae,
|
||||
|
||||
// conversion start commands:
|
||||
CONVERT_D2_OSR_1024 = 0x54,
|
||||
CONVERT_D1_OSR_1024 = 0x44,
|
||||
};
|
||||
|
||||
enum class State : uint8_t {
|
||||
COLD = 5,
|
||||
COLD_WAIT = 6,
|
||||
|
||||
UNINITIALISED = 7,
|
||||
|
||||
RUNNING = 17,
|
||||
|
||||
RESET_START = 27,
|
||||
RESET_WAIT = 28,
|
||||
|
||||
CONVERSION_D1_START = 37,
|
||||
CONVERSION_D1_WAIT = 38,
|
||||
|
||||
CONVERSION_D2_START = 47,
|
||||
CONVERSION_D2_WAIT = 48,
|
||||
};
|
||||
|
||||
State state = State::COLD;
|
||||
|
||||
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 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
|
||||
Loading…
Reference in New Issue