Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: correct SMBus block reads

Browse Source
This commit is contained in:
Peter Barker 2021-01-05 13:59:19 +11:00 committed by Peter Barker
parent 487f36146d
commit b6833eee9b
6 changed files with 142 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/SITL/SIM_BattMonitor_SMBus.cpp
View File

@ -3,7 +3,7 @@
#include <AP_Stats/AP_Stats.h>
SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
I2CRegisters_16Bit()
SMBusDevice()
{
add_register("Temperature", SMBusBattDevReg::TEMP, O_RDONLY);
add_register("Voltage", SMBusBattDevReg::VOLTAGE, O_RDONLY);
@ -13,7 +13,8 @@ SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
add_register("Cycle_Count", SMBusBattDevReg::CYCLE_COUNT, O_RDONLY);
add_register("Specification Info", SMBusBattDevReg::SPECIFICATION_INFO, O_RDONLY);
add_register("Serial", SMBusBattDevReg::SERIAL, O_RDONLY);
add_register("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, O_RDONLY);
add_block("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, O_RDONLY);
add_block("Device Name", SMBusBattDevReg::DEVICE_NAME, O_RDONLY);
add_register("Manufacture Data", SMBusBattDevReg::MANUFACTURE_DATA, O_RDONLY);
set_register(SMBusBattDevReg::TEMP, (int16_t)((15 + 273.15)*10));
@ -43,25 +44,15 @@ SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
set_register(SMBusBattDevReg::SERIAL, (uint16_t)12345);
// Set MANUFACTURE_NAME
const char *manufacturer_name = "ArduPilot";
set_register(SMBusBattDevReg::MANUFACTURE_NAME, uint16_t(manufacturer_name[0]<<8|strlen(manufacturer_name)));
uint8_t i = 1; // already sent the first byte out....
while (i < strlen(manufacturer_name)) {
const uint8_t a = manufacturer_name[i++];
uint8_t b = 0;
if (i < strlen(manufacturer_name)) {
b = manufacturer_name[i];
}
i++;
const uint16_t value = b<<8 | a;
add_register("Name", SMBusBattDevReg::MANUFACTURE_NAME + i/2, O_RDONLY);
set_register(SMBusBattDevReg::MANUFACTURE_NAME + i/2, value);
}
set_block(SMBusBattDevReg::MANUFACTURE_NAME, manufacturer_name);
const char *device_name = "SITLBatMon_V0.99";
set_block(SMBusBattDevReg::DEVICE_NAME, device_name);
// TODO: manufacturer data
}
void SITL::SIM_BattMonitor_SMBus::update(const class Aircraft &aircraft)
{
const uint32_t now = AP_HAL::millis();

11
libraries/SITL/SIM_BattMonitor_SMBus.h
View File

@ -1,10 +1,10 @@
#include "SIM_I2CDevice.h"
#include "SIM_SMBusDevice.h"
#pragma once
namespace SITL {
class SMBusBattDevReg : public I2CRegEnum {
class SMBusBattDevReg : public SMBusRegEnum {
public:
static const uint8_t TEMP = 0x08; // Temperature
static const uint8_t VOLTAGE = 0x09; // Voltage
@ -15,10 +15,11 @@ public:
static const uint8_t SPECIFICATION_INFO = 0x1A; // Specification Info
static const uint8_t SERIAL = 0x1C; // Serial Number
static const uint8_t MANUFACTURE_NAME = 0x20; // Manufacture Name
static const uint8_t DEVICE_NAME = 0x21; // Device Name
static const uint8_t MANUFACTURE_DATA = 0x23; // Manufacture Data
};
class SIM_BattMonitor_SMBus : public I2CDevice, protected I2CRegisters_16Bit
class SIM_BattMonitor_SMBus : public SMBusDevice
{
public:
@ -26,10 +27,6 @@ public:
virtual void update(const class Aircraft &aircraft) override;
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
return I2CRegisters_16Bit::rdwr(data);
}
private:
uint32_t last_update_ms;

3
libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp
View File

@ -16,6 +16,7 @@ void SITL::Rotoye::update(const class Aircraft &aircraft)
const uint32_t now = AP_HAL::millis();
if (now - last_temperature_update_ms > 1000) {
last_temperature_update_ms = now;
set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(be16toh(word[SMBusBattRotoyeDevReg::TEMP]) + 100)); // it's a little warmer inside.... (10 degrees here)
int16_t outside_temp = get_reg_value(SMBusBattRotoyeDevReg::TEMP);
set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(outside_temp + 100)); // it's a little warmer inside.... (10 degrees here)
}
}

4
libraries/SITL/SIM_I2CDevice.h
View File

@ -30,6 +30,10 @@ public:
void set_register(uint8_t reg, uint16_t value);
void set_register(uint8_t reg, int16_t value);
uint16_t get_reg_value(uint8_t reg) {
return be16toh(word[reg]);
}
protected:
uint16_t word[256];

67
libraries/SITL/SIM_SMBusDevice.cpp Normal file
View File

@ -0,0 +1,67 @@
#include "SIM_SMBusDevice.h"
int SITL::SMBusDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
{
// see if this is a block...
const uint8_t addr = data->msgs[0].buf[0];
if (blockname[addr] == nullptr) {
// not a block
return I2CRegisters_16Bit::rdwr(data);
}
// it is a block.
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");
}
data->msgs[1].buf[0] = value_lengths[addr];
const uint8_t to_copy = MIN(data->msgs[1].len-1, value_lengths[addr]);
memcpy(&data->msgs[1].buf[1], values[addr], to_copy);
data->msgs[1].len = to_copy + 1;
return 0;
}
if (data->nmsgs == 1) {
// data write request
if (data->msgs[0].flags != 0) {
AP_HAL::panic("Unexpected flags");
}
AP_HAL::panic("block writes not implemented");
}
return -1;
}
void SITL::SMBusDevice::add_block(const char *name, uint8_t reg, int8_t mode)
{
// ::fprintf(stderr, "Adding block %u (0x%02x) (%s)\n", reg, reg, name);
blockname[reg] = name;
if (mode == O_RDONLY || mode == O_RDWR) {
readable_blocks.set((uint8_t)reg);
}
if (mode == O_WRONLY || mode == O_RDWR) {
writable_blocks.set((uint8_t)reg);
}
}
void SITL::SMBusDevice::set_block(uint8_t block, uint8_t *value, uint8_t valuelen)
{
if (blockname[block] == nullptr) {
AP_HAL::panic("Setting un-named block %u", block);
}
// ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
if (valuelen == 0) {
AP_HAL::panic("Zero-length values not permitted by spec");
}
if (values[block] != nullptr) {
free(values[block]);
}
values[block] = (char*)malloc(valuelen);
memcpy(values[block], value, valuelen);
value_lengths[block] = valuelen;
}

57
libraries/SITL/SIM_SMBusDevice.h Normal file
View File

@ -0,0 +1,57 @@
#pragma once
#include "SIM_I2CDevice.h"
namespace SITL {
class SMBusRegEnum : public I2CRegEnum {
};
class SMBusDevice : public I2CDevice, private I2CRegisters_16Bit {
public:
SMBusDevice() :
I2CDevice(),
I2CRegisters_16Bit()
{ }
void set_register(uint8_t reg, uint16_t value) {
I2CRegisters_16Bit::set_register(reg, value);
}
void set_register(uint8_t reg, int16_t value) {
I2CRegisters_16Bit::set_register(reg, value);
}
uint16_t get_reg_value(uint8_t reg) {
return I2CRegisters_16Bit::get_reg_value(reg);
}
protected:
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
void set_block(uint8_t block, uint8_t *value, uint8_t valuelen);
void add_block(const char *name, uint8_t reg, int8_t mode);
void set_block(uint8_t block, const char *value) {
set_block(block, (uint8_t*)value, strlen(value));
}
void add_register(const char *name, uint8_t reg, int8_t mode) {
I2CRegisters_16Bit::add_register(name, reg, mode);
}
private:
const char *blockname[256];
Bitmask<256> writable_blocks;
Bitmask<256> readable_blocks;
// 256 pointers-to-malloced-values:
char *values[256];
uint8_t value_lengths[256];
};
} // namespace SITL