From 8df29a3960e82f219402ee578562b65e03893480 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 11 Feb 2025 18:43:22 +1100 Subject: [PATCH] SITL: add simulated SHT3x temperature sensor --- libraries/SITL/SIM_I2C.cpp | 10 +- libraries/SITL/SIM_Temperature_SHT3x.cpp | 153 +++++++++++++++++++++++ libraries/SITL/SIM_Temperature_SHT3x.h | 96 ++++++++++++++ libraries/SITL/SIM_config.h | 4 + 4 files changed, 262 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_Temperature_SHT3x.cpp create mode 100644 libraries/SITL/SIM_Temperature_SHT3x.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 8d0bfd55bf..40e1d1a89b 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -21,11 +21,12 @@ #include #include +#include "SIM_I2C.h" + #include "SIM_Airspeed_DLVR.h" #include "SIM_BattMonitor_SMBus_Generic.h" #include "SIM_BattMonitor_SMBus_Maxell.h" #include "SIM_BattMonitor_SMBus_Rotoye.h" -#include "SIM_I2C.h" #include "SIM_ICM40609.h" #include "SIM_INA3221.h" #include "SIM_IS31FL3195.h" @@ -36,6 +37,7 @@ #include "SIM_MS5611.h" #include "SIM_QMC5883L.h" #include "SIM_Temperature_MCP9600.h" +#include "SIM_Temperature_SHT3x.h" #include "SIM_Temperature_TSYS01.h" #include "SIM_Temperature_TSYS03.h" #include "SIM_TeraRangerI2C.h" @@ -75,6 +77,9 @@ static SIM_BattMonitor_SMBus_Generic smbus_generic; #if AP_SIM_AIRSPEED_DLVR_ENABLED static Airspeed_DLVR airspeed_dlvr; #endif +#if AP_SIM_TEMPERATURE_SHT3X_ENABLED +static SHT3x sht3x; +#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED #if AP_SIM_TEMPERATURE_TSYS01_ENABLED static TSYS01 tsys01; #endif @@ -133,6 +138,9 @@ struct i2c_device_at_address { #if AP_SIM_ICM40609_ENABLED { 1, 0x01, icm40609 }, #endif +#if AP_SIM_TEMPERATURE_SHT3X_ENABLED + { 1, 0x44, sht3x }, +#endif #if AP_SIM_TOSHIBALED_ENABLED { 1, 0x55, toshibaled }, #endif diff --git a/libraries/SITL/SIM_Temperature_SHT3x.cpp b/libraries/SITL/SIM_Temperature_SHT3x.cpp new file mode 100644 index 0000000000..6ca9025279 --- /dev/null +++ b/libraries/SITL/SIM_Temperature_SHT3x.cpp @@ -0,0 +1,153 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_SHT3X_ENABLED + +#include "SIM_Temperature_SHT3x.h" + +#include + +using namespace SITL; + +SHT3x::SHT3x() +{ + reset(); +} + +void SHT3x::reset() +{ + status.alert_pending = 1; + status.reset_detected = 1; +} + +void SHT3x::pack_reading(SITL::I2C::i2c_msg &msg) +{ + if (msg.len != 6) { + AP_HAL::panic("Unexpected length"); + } + + const uint16_t h_deconverted = (measurement.humidity*0.01) * 65535; + const uint16_t t_deconverted = (((measurement.temperature + 45) / 175) * 65535); + + msg.buf[0] = t_deconverted >> 8; + msg.buf[1] = t_deconverted & 0xff; + msg.buf[2] = crc8_generic(&msg.buf[0], 2, 0x31, 0xff); + msg.buf[3] = h_deconverted >> 8; + msg.buf[4] = h_deconverted & 0xff; + msg.buf[5] = crc8_generic(&msg.buf[3], 2, 0x31, 0xff); +} + +int SHT3x::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + // 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 uint16_t command = data->msgs[0].buf[0] << 8 | data->msgs[0].buf[1]; + switch ((Command)command) { + case Command::RESET: + case Command::CLEAR_STATUS: + case Command::MEASURE: + AP_HAL::panic("Command is write-only?!"); + case Command::READ_STATUS: + data->msgs[1].buf[0] = status.value >> 16; + data->msgs[1].buf[1] = status.value & 0xff; + data->msgs[1].len = 2; + return 0; + case Command::READ_SN: + if (data->msgs[1].len != 6) { + AP_HAL::panic("Unexpwcted SN length"); + } + for (uint8_t i=0; i<6; i++) { + data->msgs[1].buf[i] = i; + } + return 0; + AP_HAL::panic("Bad command 0x%02x", (uint8_t)command); + } + } + + if (data->nmsgs == 1 && data->msgs[0].flags == I2C_M_RD) { + // driver is attempting to retrieve a measurement + switch (state) { + case State::UNKNOWN: + case State::RESET: + case State::CLEAR_STATUS: + case State::IDLE: + AP_HAL::panic("Bad state for measurement retrieval"); + case State::MEASURING: + AP_HAL::panic("Too soon for measurement (15.5ms for high repeatability, table 4)"); + case State::MEASURED: + pack_reading(data->msgs[0]); + return 0; + } + AP_HAL::panic("Bad state"); + } + + if (data->nmsgs == 1) { + // incoming write-only command + const auto &msg = data->msgs[0]; + const uint16_t cmd = msg.buf[0] << 8 | msg.buf[1]; + switch ((Command)cmd) { + case Command::RESET: + set_state(State::RESET); + return 0; + case Command::READ_SN: + case Command::READ_STATUS: + AP_HAL::panic("Should not get here"); + case Command::CLEAR_STATUS: + set_state(State::CLEAR_STATUS); + return 0; + case Command::MEASURE: + set_state(State::MEASURING); + return 0; + } + AP_HAL::panic("Bad command 0x%02x", (uint8_t)cmd); + } + return -1; +} + +void SHT3x::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::CLEAR_STATUS: + if (time_in_state_ms() < 1) { + return; + } + status.value = 0; + status.command_status = 1; // cleared succesfully. Irony. + set_state(State::IDLE); + break; + case State::MEASURING: + if (time_in_state_ms() > 16) { + measurement.temperature = get_sim_temperature(); + measurement.humidity = 33.3; + set_state(State::MEASURED); + } + break; + case State::MEASURED: + break; + case State::IDLE: + break; + } +} + +float SHT3x::get_sim_temperature() const +{ + float sim_alt = AP::sitl()->state.altitude; + sim_alt += 2 * rand_float(); + + // To Do: Add a sensor board temperature offset parameter + return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25; +} + +#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED diff --git a/libraries/SITL/SIM_Temperature_SHT3x.h b/libraries/SITL/SIM_Temperature_SHT3x.h new file mode 100644 index 0000000000..5ac4c8e37f --- /dev/null +++ b/libraries/SITL/SIM_Temperature_SHT3x.h @@ -0,0 +1,96 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_SHT3X_ENABLED + +#include "SIM_I2CDevice.h" + +/* + Simulator for the SHT3X temperature sensor + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --speedup=1 + +param set TEMP1_TYPE 8 +param set TEMP1_ADDR 0x44 + +*/ + +namespace SITL { + +class SHT3x : public I2CDevice +{ +public: + + SHT3x(); + + void update(const class Aircraft &aircraft) override; + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + +private: + + // should be a call on aircraft: + float last_temperature = -1000.0f; + + enum class State { + UNKNOWN = 22, + RESET = 23, + CLEAR_STATUS = 24, + IDLE = 25, + MEASURING = 26, + MEASURED = 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; + + float temperature_for_adc(uint32_t adc) const; + uint32_t calculate_adc(float temperature) const; + uint32_t adc; + + void pack_reading(SITL::I2C::i2c_msg &msg); + + enum class Command { + RESET = 0x30A2, + READ_STATUS = 0xF32D, + CLEAR_STATUS = 0x3041, + MEASURE = 0x2C06, // clock-stretching-enabled, high-repatability + READ_SN = 0x3780, + }; + + union { + struct { + uint8_t checksum_status : 1; + uint8_t command_status : 1; + uint8_t reserved1 : 2; + uint8_t reset_detected : 1; + uint8_t reserved2 : 5; + uint8_t T_tracking_alert : 1; + uint8_t RH_tracking_alert : 1; + uint8_t reserved3 : 1; + uint8_t header_status : 1; + uint8_t reserved4 : 1; + uint8_t alert_pending : 1; + }; + uint16_t value; + } status; + + struct { + float temperature; + float humidity; + } measurement; + + void reset(); +}; + +} // namespace SITL + +#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 74dba060ce..d13bcc454d 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -192,6 +192,10 @@ #define AP_SIM_TEMPERATURE_MCP9600_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_TEMPERATURE_SHT3X_ENABLED +#define AP_SIM_TEMPERATURE_SHT3X_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_TEMPERATURE_TSYS01_ENABLED #define AP_SIM_TEMPERATURE_TSYS01_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif