From a39fe7768177e333b40b2b29be539c49204d4a37 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 17 Oct 2021 09:33:49 +1100 Subject: [PATCH] SITL: add simulated MCP9600 --- libraries/SITL/SIM_I2C.cpp | 3 + libraries/SITL/SIM_I2CDevice.cpp | 142 ++++++++++++++++++++- libraries/SITL/SIM_I2CDevice.h | 27 ++++ libraries/SITL/SIM_Temperature_MCP9600.cpp | 48 +++++++ libraries/SITL/SIM_Temperature_MCP9600.h | 38 ++++++ 5 files changed, 253 insertions(+), 5 deletions(-) create mode 100644 libraries/SITL/SIM_Temperature_MCP9600.cpp create mode 100644 libraries/SITL/SIM_Temperature_MCP9600.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 96fb99c5aa..1ef5749444 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -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 }, diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp index 67f7e6547f..0238eb27d1 100644 --- a/libraries/SITL/SIM_I2CDevice.cpp +++ b/libraries/SITL/SIM_I2CDevice.cpp @@ -1,9 +1,20 @@ #include "SIM_I2CDevice.h" #include +#ifndef HAL_DEBUG_I2DEVICE +#define HAL_DEBUG_I2DEVICE 0 +#endif + +#if HAL_DEBUG_I2DEVICE +#include +#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]; +} diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index 7faf58444d..4e193d9ec2 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/libraries/SITL/SIM_I2CDevice.h @@ -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 { diff --git a/libraries/SITL/SIM_Temperature_MCP9600.cpp b/libraries/SITL/SIM_Temperature_MCP9600.cpp new file mode 100644 index 0000000000..dd3b056a49 --- /dev/null +++ b/libraries/SITL/SIM_Temperature_MCP9600.cpp @@ -0,0 +1,48 @@ +#include "SIM_Temperature_MCP9600.h" + +using namespace SITL; + +#include +#include + +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); +} diff --git a/libraries/SITL/SIM_Temperature_MCP9600.h b/libraries/SITL/SIM_Temperature_MCP9600.h new file mode 100644 index 0000000000..29b60a3d6e --- /dev/null +++ b/libraries/SITL/SIM_Temperature_MCP9600.h @@ -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