Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Peter Barker 2024-10-06 12:42:35 +11:00 committed by Peter Barker
parent 0fa3086a93
commit 0d74bb1a76
3 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_I2CDevice.cpp
View File

@ -242,7 +242,7 @@ void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint8_t va
if (reg_data_len[reg] != 1) { if (reg_data_len[reg] != 1) {
AP_HAL::panic("Invalid set_register len"); 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) 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) { if (reg_data_len[reg] != 1) {
AP_HAL::panic("Invalid set_register len"); 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) int SITL::I2CRegisters_ConfigurableLength::rdwr(I2C::i2c_rdwr_ioctl_data *&data)

25
libraries/SITL/SIM_Temperature_MCP9600.cpp
View File

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

1
libraries/SITL/SIM_Temperature_MCP9600.h
View File

@ -12,6 +12,7 @@ namespace SITL {
class MCP9600DevReg : public I2CRegEnum { class MCP9600DevReg : public I2CRegEnum {
public: public:
static constexpr uint8_t HOT_JUNC { 0x00 }; 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 SENSOR_CONFIG { 0x05 };
static constexpr uint8_t WHOAMI { 0x20 }; static constexpr uint8_t WHOAMI { 0x20 };
}; };