SITL: add simulator for TSYS03 temperature sensor

libraries/SITL/SIM_I2C.cpp

@@ -28,6 +28,7 @@
 #include "SIM_BattMonitor_SMBus_Rotoye.h"
 #include "SIM_Airspeed_DLVR.h"
 #include "SIM_Temperature_TSYS01.h"
+#include "SIM_Temperature_TSYS03.h"
 #include "SIM_Temperature_MCP9600.h"
 #include "SIM_ICM40609.h"
 #include "SIM_IS31FL3195.h"
@@ -63,6 +64,9 @@ static Rotoye rotoye;
 static SIM_BattMonitor_SMBus_Generic smbus_generic;
 static Airspeed_DLVR airspeed_dlvr;
 static TSYS01 tsys01;
+#if AP_SIM_TSYS03_ENABLED
+static TSYS03 tsys03;
+#endif
 static MCP9600 mcp9600;
 static ICM40609 icm40609;
 static MS5525 ms5525;
@@ -107,6 +111,9 @@ struct i2c_device_at_address {
 #endif
 #if AP_SIM_IS31FL3195_ENABLED
     { 2, SIM_IS31FL3195_ADDR, is31fl3195 },    // IS31FL3195 RGB LED driver; see page 9
+#endif
+#if AP_SIM_TSYS03_ENABLED
+    { 2, 0x40, tsys03 },
 #endif
     { 2, 0x77, ms5611 },        // MS5611: BARO_PROBE_EXT = 2
 };

libraries/SITL/SIM_Temperature_TSYS03.cpp

@@ -0,0 +1,199 @@
+#include "SIM_config.h"
+
+#if AP_SIM_TSYS03_ENABLED
+
+#include "SIM_Temperature_TSYS03.h"
+
+#include <stdio.h>
+
+#include <GCS_MAVLink/GCS.h>
+
+constexpr const uint8_t SITL::TSYS03::serial[3];
+
+int SITL::TSYS03::rdwr_handle_read(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    // something is expecting a response....
+    if (data->msgs[0].flags != 0) {
+        AP_HAL::panic("Unexpected flags");
+    }
+    if (data->msgs[1].flags != I2C_M_RD) {
+        AP_HAL::panic("Unexpected flags");
+    }
+    const uint8_t command = data->msgs[0].buf[0];
+    switch ((Command)command) {
+    case Command::RESET:
+        AP_HAL::panic("Bad RESET");
+    case Command::READ_SERIAL: {
+        if (state != State::RESET) {
+            // not sure if this is illegal or not?
+            AP_HAL::panic("reading serial outside RESET state");
+        }
+        if (data->msgs[1].len != 4) {
+            AP_HAL::panic("Unexpected serial read length");
+        }
+        memcpy(data->msgs[1].buf, serial, 3);
+        uint8_t crc = 0;
+        for (uint8_t i=0; i<3; i++) {
+            crc = crc8_dvb(crc, serial[i], 0x31);
+        }
+        data->msgs[1].buf[3] = crc;
+        break;
+    }
+    case Command::CONVERT:
+        AP_HAL::panic("Bad CONVERT");
+    case Command::READ_ADC: {
+        if (data->msgs[1].len != 3) {
+            AP_HAL::panic("Unexpected adc read length");
+        }
+        if (state == State::CONVERTING) {
+            // we've been asked for values while still converting.
+            // Return zeroes
+        } else if (state == State::CONVERTED) {
+            data->msgs[1].buf[1] = adc & 0xff;
+            data->msgs[1].buf[0] = adc >> 8;
+
+            uint8_t crc = 0;
+            for (uint8_t i=0; i<2; i++) {
+                crc = crc8_dvb(crc, data->msgs[1].buf[i], 0x31);
+            }
+            data->msgs[1].buf[2] = crc;
+
+            set_state(State::IDLE);
+        } else {
+            // AP_HAL::panic("READ_ADC in bad state");
+            // this happens at startup
+            return -1;
+        }
+        break;
+    }
+    }
+    return 0;
+}
+
+int SITL::TSYS03::rdwr_handle_write(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    // incoming write-only command
+    const auto &msg = data->msgs[0];
+    const uint8_t cmd = msg.buf[0];
+
+    switch ((Command)cmd) {
+    case Command::RESET:
+        set_state(State::RESET);
+        break;
+    case Command::READ_SERIAL:
+        AP_HAL::panic("bad serial read");
+    case Command::CONVERT:
+        if (state != State::RESET &&
+            state != State::CONVERTING &&
+            state != State::IDLE &&
+            state != State::READ_SERIAL) {
+            AP_HAL::panic("Convert outside reset/idle");
+        }
+        set_state(State::CONVERTING);
+        break;
+    case Command::READ_ADC:
+        AP_HAL::panic("bad READ_ADC");
+    }
+    return 0;
+}
+
+int SITL::TSYS03::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    if (data->nmsgs == 2) {
+        return rdwr_handle_read(data);
+    }
+
+    if (data->nmsgs == 1) {
+        return rdwr_handle_write(data);
+    }
+    return -1;
+}
+
+// swiped from the driver:
+float SITL::TSYS03::temperature_for_adc(const uint16_t _adc) const
+{
+    const float temperature = -40.0 + _adc * 165 / (powf(2, 16) - 1.0);
+
+    return temperature;
+}
+
+uint16_t SITL::TSYS03::calculate_adc(float temperature) const
+{
+    // bisect to find the adc24 value:
+    uint16_t min_adc = 0;
+    uint16_t max_adc = 0xffff;
+    uint16_t current_adc = (min_adc+(uint64_t)max_adc)/2;
+    float current_error = fabsf(temperature_for_adc(current_adc) - temperature);
+    bool bisect_down = false;
+
+    // temperature_for_adc(9378708);  // should be 10.59
+
+    while (labs(int32_t(max_adc - min_adc)) > 1 && current_error > 0.05) {
+        uint16_t candidate_adc;
+        if (bisect_down) {
+            candidate_adc = (min_adc+(uint64_t)current_adc)/2;
+        } else {
+            candidate_adc = (max_adc+(uint64_t)current_adc)/2;
+        }
+        const float candidate_temp = temperature_for_adc(candidate_adc);
+        const float candidate_error = fabsf(candidate_temp - temperature);
+        if (candidate_error > current_error) {
+            // worse result
+            if (bisect_down) {
+                min_adc = candidate_adc;
+                bisect_down = false;
+            } else {
+                max_adc = candidate_adc;
+                bisect_down = true;
+            }
+        } else {
+            // better result
+            if (bisect_down) {
+                max_adc = current_adc;
+                bisect_down = false;
+            } else {
+                min_adc = current_adc;
+                bisect_down = true;
+            }
+            current_adc = candidate_adc;
+            current_error = candidate_error;
+        }
+    }
+    return current_adc;
+}
+
+void SITL::TSYS03::update(const class Aircraft &aircraft)
+{
+    switch (state) {
+    case State::UNKNOWN:
+        break;
+    case State::RESET:
+        if (time_in_state_ms() > 2) {
+            set_state(State::IDLE);
+        }
+        break;
+    case State::READ_SERIAL:
+        break;
+    case State::IDLE:
+        break;
+    case State::CONVERTING:
+        if (time_in_state_ms() > 5) {
+            const float temperature = get_sim_temperature(aircraft);
+            if (!is_equal(last_temperature, temperature)) {
+                last_temperature = temperature;
+                adc = calculate_adc(KELVIN_TO_C(temperature));
+            }
+            set_state(State::CONVERTED);
+        }
+        break;
+    case State::CONVERTED:
+        break;
+    }
+}
+
+float SITL::TSYS03::get_sim_temperature(const Aircraft &aircraft) const
+{
+    return aircraft.get_battery_temperature();
+}
+
+#endif  // AP_SIM_TSYS03_ENABLED

libraries/SITL/SIM_Temperature_TSYS03.h

@@ -0,0 +1,67 @@
+#include "SIM_config.h"
+
+#if AP_SIM_TSYS03_ENABLED
+
+#include "SIM_I2CDevice.h"
+
+/*
+  Simulator for the TSYS03 temperature sensor
+
+*/
+
+namespace SITL {
+
+class TSYS03 : public I2CDevice
+{
+public:
+
+    void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+
+private:
+
+    int rdwr_handle_read(I2C::i2c_rdwr_ioctl_data *&data);
+    int rdwr_handle_write(I2C::i2c_rdwr_ioctl_data *&data);
+
+    // should be a call on aircraft:
+    float last_temperature = -1000.0f;
+
+    enum class State {
+        UNKNOWN = 22,
+        RESET = 23,
+        READ_SERIAL = 24,
+        IDLE = 25,
+        CONVERTING = 26,
+        CONVERTED = 27,
+    } state = State::RESET;
+
+    uint32_t state_start_time_ms;
+
+    void set_state(State new_state) {
+        state = new_state;
+        state_start_time_ms = AP_HAL::millis();
+    }
+    uint32_t time_in_state_ms() const {
+        return AP_HAL::millis() - state_start_time_ms;
+    }
+
+    float get_sim_temperature(const class Aircraft &aircraft) const;
+
+    float temperature_for_adc(uint16_t adc) const;
+    uint16_t calculate_adc(float temperature) const;
+    uint32_t adc;
+
+    enum class Command {
+        RESET       = 0x1E,
+        READ_SERIAL = 0x0A,
+        CONVERT     = 0x46,
+        READ_ADC    = 0x00,
+    };
+
+    static constexpr uint8_t serial[] { 0xAB, 0xCD, 0xEF };
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_TSYS03_ENABLED

libraries/SITL/SIM_config.h

@@ -25,3 +25,7 @@
 #ifndef AP_SIM_SHIP_ENABLED
 #define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
+
+#ifndef AP_SIM_TSYS03_ENABLED
+#define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif