SITL: add simulated MCP9600

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 },

libraries/SITL/SIM_I2CDevice.cpp

@@ -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*)&reg_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];
+}

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 {

libraries/SITL/SIM_Temperature_MCP9600.cpp

@@ -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);
+}

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