SITL: correct MCP9600 simulation

- correctly fill data-ready register - adjust for different register configuration the driver shoves in - correct WHOAMI register length - correct 8-bit register reads in variable-length-register i2c simulation
ArduPilot · Oct 6, 2024 · 42f3955 · 42f3955
1 parent 8fce3eb
commit 42f3955
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 5 deletions.
diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp
@@ -242,7 +242,7 @@ void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint8_t va
     if (reg_data_len[reg] != 1) {
         AP_HAL::panic("Invalid set_register len");
     }
-    reg_data[reg] = value;
+    reg_data[reg] = value << 24;
 }
 
 void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t value)
@@ -254,7 +254,7 @@ void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t val
     if (reg_data_len[reg] != 1) {
         AP_HAL::panic("Invalid set_register len");
     }
-    reg_data[reg] = value;
+    reg_data[reg] = value << 24;
 }
 
 int SITL::I2CRegisters_ConfigurableLength::rdwr(I2C::i2c_rdwr_ioctl_data *&data)

diff --git a/libraries/SITL/SIM_Temperature_MCP9600.cpp b/libraries/SITL/SIM_Temperature_MCP9600.cpp
@@ -5,12 +5,19 @@ using namespace SITL;
 #include <GCS_MAVLink/GCS.h>
 #include <signal.h>
 
+enum class MCP9600StatusBits {
+    TH_UPDATE = (1 << 6)  // data ready to read
+};
+
 void MCP9600::init()
 {
     set_debug(true);
 
-    add_register("WHOAMI", MCP9600DevReg::WHOAMI, 2, I2CRegisters::RegMode::RDONLY);
-    set_register(MCP9600DevReg::WHOAMI, (uint16_t)0x40);
+    add_register("WHOAMI", MCP9600DevReg::WHOAMI, 1, I2CRegisters::RegMode::RDONLY);
+    set_register(MCP9600DevReg::WHOAMI, (uint8_t)0x40);
+
+    add_register("SENSOR_STATUS", MCP9600DevReg::SENSOR_STATUS, 1, I2CRegisters::RegMode::RDWR);
+    set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)0x00);
 
     add_register("SENSOR_CONFIG", MCP9600DevReg::SENSOR_CONFIG, 1, I2CRegisters::RegMode::RDWR);
     set_register(MCP9600DevReg::SENSOR_CONFIG, (uint8_t)0x00);
@@ -32,14 +39,26 @@ void MCP9600::update(const class Aircraft &aircraft)
         // unconfigured; FIXME lack of fidelity
         return;
     }
-    if ((config & 0b111) != 1) {  // FIXME: this is just the default config
+    if ((config & 0b111) != 0b10) {
         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");
     }
+
+    // dont update if we already have data ready to read (CHECKME: fidelity)
+    uint8_t status = 0;
+    get_reg_value(MCP9600DevReg::SENSOR_STATUS, config);
+
+    if (status & (uint8_t)MCP9600StatusBits::TH_UPDATE) {
+        return;
+    }
+
     static constexpr uint16_t factor = (1/0.0625);
     set_register(MCP9600DevReg::HOT_JUNC, uint16_t(htobe16(some_temperature + degrees(sinf(now_ms)) * factor)));
+
+    // indicate we have data ready to read
+    set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)MCP9600StatusBits::TH_UPDATE);
 }
 
 int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data)

diff --git a/libraries/SITL/SIM_Temperature_MCP9600.h b/libraries/SITL/SIM_Temperature_MCP9600.h
@@ -12,6 +12,7 @@ namespace SITL {
 class MCP9600DevReg : public I2CRegEnum {
 public:
     static constexpr uint8_t HOT_JUNC        { 0x00 };
+    static constexpr uint8_t SENSOR_STATUS   { 0x04 };
     static constexpr uint8_t SENSOR_CONFIG   { 0x05 };
     static constexpr uint8_t WHOAMI          { 0x20 };
 };