2020-11-11 00:03:49 -04:00
|
|
|
#include "SIM_BattMonitor_SMBus_Rotoye.h"
|
2020-11-12 04:43:29 -04:00
|
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
2020-11-11 00:03:49 -04:00
|
|
|
|
|
|
|
SITL::Rotoye::Rotoye() :
|
|
|
|
SIM_BattMonitor_SMBus_Generic()
|
|
|
|
{
|
2021-10-11 02:04:21 -03:00
|
|
|
add_register("External Temperature", SMBusBattRotoyeDevReg::TEMP_EXT, SITL::I2CRegisters::RegMode::RDONLY);
|
2020-11-11 00:03:49 -04:00
|
|
|
|
|
|
|
set_register(SMBusBattRotoyeDevReg::SERIAL, (uint16_t)39);
|
2021-03-16 02:02:25 -03:00
|
|
|
|
|
|
|
const char *manufacturer_name = "Rotoye";
|
|
|
|
set_block(SMBusBattDevReg::MANUFACTURE_NAME, manufacturer_name);
|
|
|
|
|
|
|
|
const char *device_name = "SITL_BatMon v4.03";
|
|
|
|
set_block(SMBusBattDevReg::DEVICE_NAME, device_name);
|
|
|
|
|
|
|
|
set_register(SMBusBattGenericDevReg::SERIAL, (uint16_t) 278);
|
2020-11-11 00:03:49 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void SITL::Rotoye::update(const class Aircraft &aircraft)
|
|
|
|
{
|
|
|
|
SIM_BattMonitor_SMBus_Generic::update(aircraft);
|
|
|
|
|
|
|
|
const uint32_t now = AP_HAL::millis();
|
|
|
|
if (now - last_temperature_update_ms > 1000) {
|
|
|
|
last_temperature_update_ms = now;
|
2021-01-04 22:59:19 -04:00
|
|
|
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)
|
2020-11-11 00:03:49 -04:00
|
|
|
}
|
|
|
|
}
|