aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: add I2CRegister::8Bit for 8-bit-quantity devices

Browse Source
This commit is contained in:
Peter Barker 2020-11-12 15:18:50 +11:00 committed by Peter Barker
parent 29b913cd41
commit e21e55ba51
2 changed files with 64 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/SITL/SIM_I2CDevice.cpp
View File

@ -64,6 +64,7 @@ int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
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);
@ -75,3 +76,52 @@ int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
return -1;
};
int SITL::I2CRegisters_8Bit::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_base_addr = data->msgs[0].buf[0];
uint8_t bytes_copied = 0;
while (bytes_copied < data->msgs[1].len) {
const uint8_t reg_addr = reg_base_addr + bytes_copied;
if (!readable_registers.get(reg_addr)) {
// ::printf("Register 0x%02x is not readable!\n", reg_addr);
return -1;
}
const uint8_t register_value = byte[reg_addr];
data->msgs[1].buf[bytes_copied++] = register_value;
}
data->msgs[1].len = bytes_copied;
return 0;
}
if (data->nmsgs == 1) {
// data write request
if (data->msgs[0].flags != 0) {
AP_HAL::panic("Unexpected flags");
}
const uint8_t reg_base_addr = data->msgs[0].buf[0];
uint8_t bytes_copied = 0;
while (bytes_copied < data->msgs[0].len-1) {
const uint8_t reg_addr = reg_base_addr + bytes_copied;
if (!writable_registers.get(reg_addr)) {
AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
}
const uint8_t register_value = data->msgs[0].buf[1+bytes_copied];
byte[reg_addr] = register_value;
bytes_copied++;
}
return 0;
}
return -1;
};

14
libraries/SITL/SIM_I2CDevice.h
View File

@ -35,6 +35,20 @@ protected:
uint16_t word[256];
};
class I2CRegisters_8Bit : public I2CRegisters {
public:
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
void set_register(uint8_t reg, uint8_t value);
void set_register(uint8_t reg, int8_t value);
uint8_t get_register(uint8_t num) {
return byte[(uint8_t)num];
}
protected:
uint8_t byte[256];
};
class I2CDevice {
public: