mirror of https://github.com/ArduPilot/ardupilot
49 lines
1.5 KiB
C++
49 lines
1.5 KiB
C++
|
#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);
|
||
|
|
}
|