You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
2.7 KiB
67 lines
2.7 KiB
#include "SIM_BattMonitor_SMBus.h" |
|
|
|
#include <AP_Stats/AP_Stats.h> |
|
|
|
SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() : |
|
SMBusDevice() |
|
{ |
|
add_register("Temperature", SMBusBattDevReg::TEMP, O_RDONLY); |
|
add_register("Voltage", SMBusBattDevReg::VOLTAGE, O_RDONLY); |
|
add_register("Current", SMBusBattDevReg::CURRENT, O_RDONLY); |
|
add_register("Remaining Capacity", SMBusBattDevReg::REMAINING_CAPACITY, O_RDONLY); |
|
add_register("Full Charge Capacity", SMBusBattDevReg::FULL_CHARGE_CAPACITY, O_RDONLY); |
|
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_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 + C_TO_KELVIN)*10)); |
|
// see update for voltage |
|
// see update for current |
|
// TODO: remaining capacity |
|
// TODO: full capacity |
|
set_register(SMBusBattDevReg::CYCLE_COUNT, (uint16_t(39U))); |
|
|
|
// Set SPECIFICATION_INFO |
|
union { |
|
struct { |
|
uint8_t revision : 4; |
|
uint8_t version: 4; |
|
uint8_t vscale: 4; |
|
uint8_t ipscale: 4; |
|
} fields; |
|
uint16_t word; |
|
} specinfo; |
|
|
|
specinfo.fields.revision = 0b0001; // version 1 |
|
// specinfo.fields.version = 0b0011; // 1.1 with PEC; TODO! |
|
specinfo.fields.version = 0b0001; // 1.0 |
|
specinfo.fields.vscale = 0b0000; // unknown... |
|
specinfo.fields.ipscale = 0b0000; // unknown... |
|
set_register(SMBusBattDevReg::SPECIFICATION_INFO, specinfo.word); |
|
|
|
set_register(SMBusBattDevReg::SERIAL, (uint16_t)12345); |
|
|
|
const char *manufacturer_name = "ArduPilot"; |
|
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(); |
|
if (now - last_update_ms > 100) { |
|
const float millivolts = AP::sitl()->state.battery_voltage * 1000.0f; |
|
set_register(SMBusBattDevReg::VOLTAGE, uint16_t(millivolts)); |
|
// FIXME: is this REALLY what the hardware will do? |
|
const int16_t current = constrain_int32(AP::sitl()->state.battery_current*-1000, -32768, 32767); |
|
set_register(SMBusBattDevReg::CURRENT, current); |
|
last_update_ms = now; |
|
} |
|
}
|
|
|