From 390b025fa01bcf9a9f0ed5aba2599f04f308fe03 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Sep 2023 12:00:13 +1000 Subject: [PATCH] SITL: add simulator for TSYS03 temperature sensor --- libraries/SITL/SIM_I2C.cpp | 7 + libraries/SITL/SIM_Temperature_TSYS03.cpp | 199 ++++++++++++++++++++++ libraries/SITL/SIM_Temperature_TSYS03.h | 67 ++++++++ libraries/SITL/SIM_config.h | 4 + 4 files changed, 277 insertions(+) create mode 100644 libraries/SITL/SIM_Temperature_TSYS03.cpp create mode 100644 libraries/SITL/SIM_Temperature_TSYS03.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 40152ab7fe..51450870d7 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/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 }; diff --git a/libraries/SITL/SIM_Temperature_TSYS03.cpp b/libraries/SITL/SIM_Temperature_TSYS03.cpp new file mode 100644 index 0000000000..d3a036b9cb --- /dev/null +++ b/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 + +#include + +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 diff --git a/libraries/SITL/SIM_Temperature_TSYS03.h b/libraries/SITL/SIM_Temperature_TSYS03.h new file mode 100644 index 0000000000..3963df7baf --- /dev/null +++ b/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 diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index ab5055b20e..bf0ebe588b 100644 --- a/libraries/SITL/SIM_config.h +++ b/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