diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp index e8d8a20aaeaa0..53bbcf317fcbe 100644 --- 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 index dd3b056a49b9e..c05e7221504fa 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.cpp +++ b/libraries/SITL/SIM_Temperature_MCP9600.cpp @@ -5,12 +5,19 @@ using namespace SITL; #include #include +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 index d8109cbaed558..c0d2f3ae1ae21 100644 --- 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 }; };