ardupilot/libraries/SITL/SIM_Temperature_MCP9600.cpp

49 lines
1.5 KiB
C++
Raw Normal View History

2021-10-16 19:33:49 -03:00
#include "SIM_Temperature_MCP9600.h"
using namespace SITL;
#include <GCS_MAVLink/GCS.h>
#include <signal.h>
void MCP9600::init()
{
set_debug(true);
add_register("WHOAMI", MCP9600DevReg::WHOAMI, 2, I2CRegisters::RegMode::RDONLY);
set_register(MCP9600DevReg::WHOAMI, (uint16_t)0x40);
add_register("SENSOR_CONFIG", MCP9600DevReg::SENSOR_CONFIG, 1, I2CRegisters::RegMode::RDWR);
set_register(MCP9600DevReg::SENSOR_CONFIG, (uint8_t)0x00);
add_register("HOT_JUNC", MCP9600DevReg::HOT_JUNC, 2, I2CRegisters::RegMode::RDONLY);
}
void MCP9600::update(const class Aircraft &aircraft)
{
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_temperature_update_ms < 100) { // 10Hz
return;
}
last_temperature_update_ms = now_ms;
uint8_t config;
get_reg_value(MCP9600DevReg::SENSOR_CONFIG, config);
if (config == 0) {
// unconfigured; FIXME lack of fidelity
return;
}
if ((config & 0b111) != 1) { // FIXME: this is just the default config
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");
}
static constexpr uint16_t factor = (1/0.0625);
set_register(MCP9600DevReg::HOT_JUNC, uint16_t(htobe16(some_temperature + degrees(sinf(now_ms)) * factor)));
}
int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
{
return I2CRegisters_ConfigurableLength::rdwr(data);
}