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) {
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)

25
libraries/SITL/SIM_Temperature_MCP9600.cpp
View File

@ -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)

1
libraries/SITL/SIM_Temperature_MCP9600.h
View File

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