Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_TEMPERATURE_MCP9600_ENABLED
       4             : 
       5             : #include "SIM_Temperature_MCP9600.h"
       6             : 
       7             : using namespace SITL;
       8             : 
       9             : #include <GCS_MAVLink/GCS.h>
      10             : #include <signal.h>
      11             : 
      12             : enum class MCP9600StatusBits {
      13             :     TH_UPDATE = (1 << 6)  // data ready to read
      14             : };
      15             : 
      16        2094 : void MCP9600::init()
      17             : {
      18        2094 :     set_debug(true);
      19             : 
      20        2094 :     add_register("WHOAMI", MCP9600DevReg::WHOAMI, 1, I2CRegisters::RegMode::RDONLY);
      21        2094 :     set_register(MCP9600DevReg::WHOAMI, (uint8_t)0x40);
      22             : 
      23        2094 :     add_register("SENSOR_STATUS", MCP9600DevReg::SENSOR_STATUS, 1, I2CRegisters::RegMode::RDWR);
      24        2094 :     set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)0x00);
      25             : 
      26        2094 :     add_register("SENSOR_CONFIG", MCP9600DevReg::SENSOR_CONFIG, 1, I2CRegisters::RegMode::RDWR);
      27        2094 :     set_register(MCP9600DevReg::SENSOR_CONFIG, (uint8_t)0x00);
      28             : 
      29        2094 :     add_register("HOT_JUNC", MCP9600DevReg::HOT_JUNC, 2, I2CRegisters::RegMode::RDONLY);
      30        2094 : }
      31             : 
      32   178759942 : void MCP9600::update(const class Aircraft &aircraft)
      33             : {
      34   178759942 :     const uint32_t now_ms = AP_HAL::millis();
      35   178759942 :     if (now_ms - last_temperature_update_ms < 100) {  // 10Hz
      36   178759942 :         return;
      37             :     }
      38     1488167 :     last_temperature_update_ms = now_ms;
      39             : 
      40             :     uint8_t config;
      41     1488167 :     get_reg_value(MCP9600DevReg::SENSOR_CONFIG, config);
      42     1488167 :     if (config == 0) {
      43             :         // unconfigured; FIXME lack of fidelity
      44     1488167 :         return;
      45             :     }
      46           0 :     if ((config & 0b111) != 0b10) {
      47           0 :         AP_HAL::panic("Unexpected filter configuration");
      48             :     }
      49           0 :     if ((config >> 4) != 0) {  // this is a K-type thermocouple, the default in the driver
      50           0 :         AP_HAL::panic("Unexpected thermocouple configuration");
      51             :     }
      52             : 
      53             :     // dont update if we already have data ready to read (CHECKME: fidelity)
      54           0 :     uint8_t status = 0;
      55           0 :     get_reg_value(MCP9600DevReg::SENSOR_STATUS, config);
      56             : 
      57           0 :     if (status & (uint8_t)MCP9600StatusBits::TH_UPDATE) {
      58           0 :         return;
      59             :     }
      60             : 
      61             :     static constexpr uint16_t factor = (1/0.0625);
      62           0 :     set_register(MCP9600DevReg::HOT_JUNC, uint16_t(htobe16(some_temperature + degrees(sinf(now_ms)) * factor)));
      63             : 
      64             :     // indicate we have data ready to read
      65           0 :     set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)MCP9600StatusBits::TH_UPDATE);
      66             : }
      67             : 
      68           0 : int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      69             : {
      70           0 :     return I2CRegisters_ConfigurableLength::rdwr(data);
      71             : }
      72             : 
      73             : #endif  // AP_SIM_TEMPERATURE_MCP9600_ENABLED