mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
SITL: add simulated MCP9600
This commit is contained in:
Peter Barker
Peter Barker
parent
40fea51f07
commit
a39fe77681
@ -27,6 +27,7 @@
|
||||
#include "SIM_BattMonitor_SMBus_Rotoye.h"
|
||||
#include "SIM_Airspeed_DLVR.h"
|
||||
#include "SIM_Temperature_TSYS01.h"
|
||||
#include "SIM_Temperature_MCP9600.h"
|
||||
#include "SIM_ICM40609.h"
|
||||
#include "SIM_MS5525.h"
|
||||
#include "SIM_MS5611.h"
|
||||
@ -55,6 +56,7 @@ static Maxell maxell;
|
||||
static Rotoye rotoye;
|
||||
static Airspeed_DLVR airspeed_dlvr;
|
||||
static TSYS01 tsys01;
|
||||
static MCP9600 mcp9600;
|
||||
static ICM40609 icm40609;
|
||||
static MS5525 ms5525;
|
||||
static MS5611 ms5611;
|
||||
@ -65,6 +67,7 @@ struct i2c_device_at_address {
|
||||
I2CDevice &device;
|
||||
} i2c_devices[] {
|
||||
{ 0, 0x70, maxsonari2cxl },
|
||||
{ 0, 0x60, mcp9600 }, // 0x60 is low address
|
||||
{ 0, 0x71, maxsonari2cxl_2 },
|
||||
{ 1, 0x01, icm40609 },
|
||||
{ 1, 0x55, toshibaled },
|
||||
|
||||
@ -1,9 +1,20 @@
|
||||
#include "SIM_I2CDevice.h"
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
#ifndef HAL_DEBUG_I2DEVICE
|
||||
#define HAL_DEBUG_I2DEVICE 0
|
||||
#endif
|
||||
|
||||
#if HAL_DEBUG_I2DEVICE
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#define DEBUG(fmt, args ...) do { if (get_debug()) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
|
||||
#else
|
||||
#define DEBUG(fmt, args ...)
|
||||
#endif
|
||||
|
||||
void SITL::I2CRegisters::add_register(const char *name, uint8_t reg, RegMode mode)
|
||||
{
|
||||
// ::fprintf(stderr, "Adding register %u (0x%02x) (%s)\n", reg, reg, name);
|
||||
DEBUG("Adding register %u (0x%02x) (%s)", reg, reg, name);
|
||||
regname[reg] = name;
|
||||
if (mode == RegMode::RDONLY || mode == RegMode::RDWR) {
|
||||
readable_registers.set((uint8_t)reg);
|
||||
@ -18,7 +29,8 @@ void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, uint16_t value)
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
// ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
|
||||
DEBUG("Setting %u (0x%02x) (%s) to 0x%02x", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value);
|
||||
|
||||
word[reg] = htobe16(value);
|
||||
}
|
||||
|
||||
@ -27,7 +39,7 @@ void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, int16_t value)
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
// ::fprintf(stderr, "Setting %s (%u) to 0x%02x (%c)\n", regname[reg], (unsigned)reg, (signed)value, value);
|
||||
DEBUG("Setting %s (%u) to 0x%02x", regname[reg], (unsigned)reg, (signed)value);
|
||||
word[reg] = htobe16(value);
|
||||
}
|
||||
|
||||
@ -84,7 +96,7 @@ void SITL::I2CRegisters_8Bit::set_register(uint8_t reg, uint8_t value)
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
// ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
|
||||
DEBUG("Setting %u (0x%02x) (%s) to 0x%02x (%c)", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
|
||||
byte[reg] = value;
|
||||
}
|
||||
|
||||
@ -93,7 +105,7 @@ void SITL::I2CRegisters_8Bit::set_register(uint8_t reg, int8_t value)
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
// ::fprintf(stderr, "Setting %s (%u) to 0x%02x (%c)\n", regname[reg], (unsigned)reg, (signed)value, value);
|
||||
DEBUG("Setting %s (%u) to 0x%02x (%c)", regname[reg], (unsigned)reg, (signed)value, value);
|
||||
byte[reg] = value;
|
||||
}
|
||||
|
||||
@ -184,3 +196,123 @@ int SITL::I2CCommandResponseDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::add_register(const char *name, uint8_t reg, uint8_t len, RegMode mode)
|
||||
{
|
||||
SITL::I2CRegisters::add_register(name, reg, mode);
|
||||
if (len > 4) {
|
||||
AP_HAL::panic("Only up to 4 bytes");
|
||||
}
|
||||
reg_data_len[reg] = len;
|
||||
}
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint16_t value)
|
||||
{
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
DEBUG("Setting %u (0x%02x) (%s) to 0x%02x", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value);
|
||||
|
||||
if (reg_data_len[reg] != 2) {
|
||||
AP_HAL::panic("Invalid set_register len");
|
||||
}
|
||||
reg_data[reg] = htobe16(value);
|
||||
}
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int16_t value)
|
||||
{
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
DEBUG("Setting %s (%u) to 0x%02x", regname[reg], (unsigned)reg, (signed)value);
|
||||
|
||||
if (reg_data_len[reg] != 2) {
|
||||
AP_HAL::panic("Invalid set_register len");
|
||||
}
|
||||
reg_data[reg] = htobe16(value);
|
||||
}
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint8_t value)
|
||||
{
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
DEBUG("Setting %u (0x%02x) (%s) to 0x%02x (%c)", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
|
||||
if (reg_data_len[reg] != 1) {
|
||||
AP_HAL::panic("Invalid set_register len");
|
||||
}
|
||||
reg_data[reg] = value;
|
||||
}
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t value)
|
||||
{
|
||||
if (regname[reg] == nullptr) {
|
||||
AP_HAL::panic("Setting un-named register %u", reg);
|
||||
}
|
||||
DEBUG("Setting %s (%u) to 0x%02x (%c)", regname[reg], (unsigned)reg, (signed)value, value);
|
||||
if (reg_data_len[reg] != 1) {
|
||||
AP_HAL::panic("Invalid set_register len");
|
||||
}
|
||||
reg_data[reg] = value;
|
||||
}
|
||||
|
||||
int SITL::I2CRegisters_ConfigurableLength::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
||||
{
|
||||
if (data->nmsgs == 2) {
|
||||
// data read request
|
||||
if (data->msgs[0].flags != 0) {
|
||||
AP_HAL::panic("Unexpected flags");
|
||||
}
|
||||
if (data->msgs[1].flags != I2C_M_RD) {
|
||||
AP_HAL::panic("Unexpected flags");
|
||||
}
|
||||
const uint8_t reg_addr = data->msgs[0].buf[0];
|
||||
if (data->msgs[1].len != reg_data_len[reg_addr]) {
|
||||
AP_HAL::panic("Invalid rdwr len");
|
||||
}
|
||||
if (!readable_registers.get(reg_addr)) {
|
||||
// ::printf("Register 0x%02x is not readable!\n", reg_addr);
|
||||
return -1;
|
||||
}
|
||||
const uint32_t register_value = reg_data[reg_addr];
|
||||
if (data->msgs[1].len == 1) {
|
||||
data->msgs[1].buf[0] = register_value >> 24;
|
||||
} else if (data->msgs[1].len == 2) {
|
||||
const uint16_t v = htobe16(register_value & 0xffff);
|
||||
memcpy(&(data->msgs[1].buf[0]), &v, 2);
|
||||
} else {
|
||||
AP_HAL::panic("Bad length"); // FIXME
|
||||
}
|
||||
data->msgs[1].len = reg_data_len[reg_addr];
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (data->nmsgs == 1) {
|
||||
// data write request
|
||||
if (data->msgs[0].flags != 0) {
|
||||
AP_HAL::panic("Unexpected flags");
|
||||
}
|
||||
// FIXME: handle multi-register writes
|
||||
const uint8_t reg_addr = data->msgs[0].buf[0];
|
||||
if (!writable_registers.get(reg_addr)) {
|
||||
AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
|
||||
}
|
||||
const uint8_t data_msg_len = data->msgs[0].len - 1;
|
||||
if (data_msg_len != reg_data_len[reg_addr]) {
|
||||
AP_HAL::panic("Invalid rdwr len");
|
||||
}
|
||||
memcpy((uint8_t*)®_data[reg_addr], &data->msgs[0].buf[1], data_msg_len);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return -1;
|
||||
};
|
||||
|
||||
void SITL::I2CRegisters_ConfigurableLength::get_reg_value(uint8_t reg, uint8_t &value) const
|
||||
{
|
||||
if (reg_data_len[reg] != 1) {
|
||||
AP_HAL::panic("Invalid reg_reg_value len");
|
||||
}
|
||||
value = reg_data[reg];
|
||||
}
|
||||
|
||||
@ -32,6 +32,33 @@ protected:
|
||||
Bitmask<256> writable_registers;
|
||||
Bitmask<256> readable_registers;
|
||||
|
||||
void set_debug(bool value) { debug = value; }
|
||||
bool get_debug() const { return debug; }
|
||||
|
||||
private:
|
||||
bool debug;
|
||||
};
|
||||
|
||||
class I2CRegisters_ConfigurableLength : public I2CRegisters {
|
||||
public:
|
||||
void add_register(const char *name, uint8_t reg, uint8_t len, RegMode mode);
|
||||
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
|
||||
void set_register(uint8_t reg, uint8_t *data, uint8_t len);
|
||||
void set_register(uint8_t reg, uint16_t value);
|
||||
void set_register(uint8_t reg, int16_t value);
|
||||
void set_register(uint8_t reg, uint8_t value);
|
||||
void set_register(uint8_t reg, int8_t value);
|
||||
|
||||
// void get_reg_value(uint8_t reg, int8_t &value) const;
|
||||
void get_reg_value(uint8_t reg, uint8_t &value) const;
|
||||
// void get_reg_value(uint8_t reg, int16_t &value) const;
|
||||
// void get_reg_value(uint8_t reg, uint16_t &value) const;
|
||||
// void get_reg_value(uint8_t reg, uint8_t *value, uint8_t len) const;
|
||||
|
||||
protected:
|
||||
|
||||
uint32_t reg_data[256]; // OK, so not *that* configurable ATM....
|
||||
uint8_t reg_data_len[256];
|
||||
};
|
||||
|
||||
class I2CRegisters_16Bit : public I2CRegisters {
|
||||
|
||||
48
libraries/SITL/SIM_Temperature_MCP9600.cpp
Normal file
48
libraries/SITL/SIM_Temperature_MCP9600.cpp
Normal file
@ -0,0 +1,48 @@
|
||||
#include "SIM_Temperature_MCP9600.h"
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <signal.h>
|
||||
|
||||
void MCP9600::init()
|
||||
{
|
||||
set_debug(true);
|
||||
|
||||
add_register("WHOAMI", MCP9600DevReg::WHOAMI, 2, I2CRegisters::RegMode::RDONLY);
|
||||
set_register(MCP9600DevReg::WHOAMI, (uint16_t)0x40);
|
||||
|
||||
add_register("SENSOR_CONFIG", MCP9600DevReg::SENSOR_CONFIG, 1, I2CRegisters::RegMode::RDWR);
|
||||
set_register(MCP9600DevReg::SENSOR_CONFIG, (uint8_t)0x00);
|
||||
|
||||
add_register("HOT_JUNC", MCP9600DevReg::HOT_JUNC, 2, I2CRegisters::RegMode::RDONLY);
|
||||
}
|
||||
|
||||
void MCP9600::update(const class Aircraft &aircraft)
|
||||
{
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - last_temperature_update_ms < 100) { // 10Hz
|
||||
return;
|
||||
}
|
||||
last_temperature_update_ms = now_ms;
|
||||
|
||||
uint8_t config;
|
||||
get_reg_value(MCP9600DevReg::SENSOR_CONFIG, config);
|
||||
if (config == 0) {
|
||||
// unconfigured; FIXME lack of fidelity
|
||||
return;
|
||||
}
|
||||
if ((config & 0b111) != 1) { // FIXME: this is just the default config
|
||||
AP_HAL::panic("Unexpected filter configuration");
|
||||
}
|
||||
if ((config >> 4) != 0) { // this is a K-type thermocouple, the default in the driver
|
||||
AP_HAL::panic("Unexpected thermocouple configuration");
|
||||
}
|
||||
static constexpr uint16_t factor = (1/0.0625);
|
||||
set_register(MCP9600DevReg::HOT_JUNC, uint16_t(htobe16(some_temperature + degrees(sinf(now_ms)) * factor)));
|
||||
}
|
||||
|
||||
int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
||||
{
|
||||
return I2CRegisters_ConfigurableLength::rdwr(data);
|
||||
}
|
||||
38
libraries/SITL/SIM_Temperature_MCP9600.h
Normal file
38
libraries/SITL/SIM_Temperature_MCP9600.h
Normal file
@ -0,0 +1,38 @@
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
/*
|
||||
Simulator for the MCP9600 temperature sensor
|
||||
|
||||
DataSheet; https://www.microchip.com/content/dam/mchp/documents/OTH/ProductDocuments/DataSheets/MCP960X-Data-Sheet-20005426.pdf
|
||||
|
||||
*/
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class MCP9600DevReg : public I2CRegEnum {
|
||||
public:
|
||||
static constexpr uint8_t HOT_JUNC { 0x00 };
|
||||
static constexpr uint8_t SENSOR_CONFIG { 0x05 };
|
||||
static constexpr uint8_t WHOAMI { 0x20 };
|
||||
};
|
||||
|
||||
class MCP9600 : public I2CDevice, private I2CRegisters_ConfigurableLength
|
||||
{
|
||||
public:
|
||||
|
||||
void init() override;
|
||||
|
||||
void update(const class Aircraft &aircraft) override;
|
||||
|
||||
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
|
||||
|
||||
private:
|
||||
|
||||
// should be a call on aircraft:
|
||||
float some_temperature = 26.5;
|
||||
float last_temperature = -1000.0f;
|
||||
|
||||
uint32_t last_temperature_update_ms;
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
Loading…
Reference in New Issue
Block a user