Skip to content

Commit 8df29a3

Browse files
peterbarkertridge
peterbarker
authored and
tridge
committed
SITL: add simulated SHT3x temperature sensor
1 parent 0ae3e7c commit 8df29a3

File tree

4 files changed

+262
-1
lines changed

4 files changed

+262
-1
lines changed

‎libraries/SITL/SIM_I2C.cpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,11 +21,12 @@
2121
#include <GCS_MAVLink/GCS.h>
2222
#include <SITL/SITL.h>
2323

24+
#include "SIM_I2C.h"
25+
2426
#include "SIM_Airspeed_DLVR.h"
2527
#include "SIM_BattMonitor_SMBus_Generic.h"
2628
#include "SIM_BattMonitor_SMBus_Maxell.h"
2729
#include "SIM_BattMonitor_SMBus_Rotoye.h"
28-
#include "SIM_I2C.h"
2930
#include "SIM_ICM40609.h"
3031
#include "SIM_INA3221.h"
3132
#include "SIM_IS31FL3195.h"
@@ -36,6 +37,7 @@
3637
#include "SIM_MS5611.h"
3738
#include "SIM_QMC5883L.h"
3839
#include "SIM_Temperature_MCP9600.h"
40+
#include "SIM_Temperature_SHT3x.h"
3941
#include "SIM_Temperature_TSYS01.h"
4042
#include "SIM_Temperature_TSYS03.h"
4143
#include "SIM_TeraRangerI2C.h"
@@ -75,6 +77,9 @@ static SIM_BattMonitor_SMBus_Generic smbus_generic;
7577
#if AP_SIM_AIRSPEED_DLVR_ENABLED
7678
static Airspeed_DLVR airspeed_dlvr;
7779
#endif
80+
#if AP_SIM_TEMPERATURE_SHT3X_ENABLED
81+
static SHT3x sht3x;
82+
#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED
7883
#if AP_SIM_TEMPERATURE_TSYS01_ENABLED
7984
static TSYS01 tsys01;
8085
#endif
@@ -133,6 +138,9 @@ struct i2c_device_at_address {
133138
#if AP_SIM_ICM40609_ENABLED
134139
{ 1, 0x01, icm40609 },
135140
#endif
141+
#if AP_SIM_TEMPERATURE_SHT3X_ENABLED
142+
{ 1, 0x44, sht3x },
143+
#endif
136144
#if AP_SIM_TOSHIBALED_ENABLED
137145
{ 1, 0x55, toshibaled },
138146
#endif

‎libraries/SITL/SIM_Temperature_SHT3x.cpp‎

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
#include "SIM_config.h"
2+
3+
#if AP_SIM_TEMPERATURE_SHT3X_ENABLED
4+
5+
#include "SIM_Temperature_SHT3x.h"
6+
7+
#include <stdio.h>
8+
9+
using namespace SITL;
10+
11+
SHT3x::SHT3x()
12+
{
13+
reset();
14+
}
15+
16+
void SHT3x::reset()
17+
{
18+
status.alert_pending = 1;
19+
status.reset_detected = 1;
20+
}
21+
22+
void SHT3x::pack_reading(SITL::I2C::i2c_msg &msg)
23+
{
24+
if (msg.len != 6) {
25+
AP_HAL::panic("Unexpected length");
26+
}
27+
28+
const uint16_t h_deconverted = (measurement.humidity*0.01) * 65535;
29+
const uint16_t t_deconverted = (((measurement.temperature + 45) / 175) * 65535);
30+
31+
msg.buf[0] = t_deconverted >> 8;
32+
msg.buf[1] = t_deconverted & 0xff;
33+
msg.buf[2] = crc8_generic(&msg.buf[0], 2, 0x31, 0xff);
34+
msg.buf[3] = h_deconverted >> 8;
35+
msg.buf[4] = h_deconverted & 0xff;
36+
msg.buf[5] = crc8_generic(&msg.buf[3], 2, 0x31, 0xff);
37+
}
38+
39+
int SHT3x::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
40+
{
41+
if (data->nmsgs == 2) {
42+
// something is expecting a response....
43+
if (data->msgs[0].flags != 0) {
44+
AP_HAL::panic("Unexpected flags");
45+
}
46+
if (data->msgs[1].flags != I2C_M_RD) {
47+
AP_HAL::panic("Unexpected flags");
48+
}
49+
const uint16_t command = data->msgs[0].buf[0] << 8 | data->msgs[0].buf[1];
50+
switch ((Command)command) {
51+
case Command::RESET:
52+
case Command::CLEAR_STATUS:
53+
case Command::MEASURE:
54+
AP_HAL::panic("Command is write-only?!");
55+
case Command::READ_STATUS:
56+
data->msgs[1].buf[0] = status.value >> 16;
57+
data->msgs[1].buf[1] = status.value & 0xff;
58+
data->msgs[1].len = 2;
59+
return 0;
60+
case Command::READ_SN:
61+
if (data->msgs[1].len != 6) {
62+
AP_HAL::panic("Unexpwcted SN length");
63+
}
64+
for (uint8_t i=0; i<6; i++) {
65+
data->msgs[1].buf[i] = i;
66+
}
67+
return 0;
68+
AP_HAL::panic("Bad command 0x%02x", (uint8_t)command);
69+
}
70+
}
71+
72+
if (data->nmsgs == 1 && data->msgs[0].flags == I2C_M_RD) {
73+
// driver is attempting to retrieve a measurement
74+
switch (state) {
75+
case State::UNKNOWN:
76+
case State::RESET:
77+
case State::CLEAR_STATUS:
78+
case State::IDLE:
79+
AP_HAL::panic("Bad state for measurement retrieval");
80+
case State::MEASURING:
81+
AP_HAL::panic("Too soon for measurement (15.5ms for high repeatability, table 4)");
82+
case State::MEASURED:
83+
pack_reading(data->msgs[0]);
84+
return 0;
85+
}
86+
AP_HAL::panic("Bad state");
87+
}
88+
89+
if (data->nmsgs == 1) {
90+
// incoming write-only command
91+
const auto &msg = data->msgs[0];
92+
const uint16_t cmd = msg.buf[0] << 8 | msg.buf[1];
93+
switch ((Command)cmd) {
94+
case Command::RESET:
95+
set_state(State::RESET);
96+
return 0;
97+
case Command::READ_SN:
98+
case Command::READ_STATUS:
99+
AP_HAL::panic("Should not get here");
100+
case Command::CLEAR_STATUS:
101+
set_state(State::CLEAR_STATUS);
102+
return 0;
103+
case Command::MEASURE:
104+
set_state(State::MEASURING);
105+
return 0;
106+
}
107+
AP_HAL::panic("Bad command 0x%02x", (uint8_t)cmd);
108+
}
109+
return -1;
110+
}
111+
112+
void SHT3x::update(const class Aircraft &aircraft)
113+
{
114+
switch (state) {
115+
case State::UNKNOWN:
116+
break;
117+
case State::RESET:
118+
if (time_in_state_ms() > 2) {
119+
set_state(State::IDLE);
120+
}
121+
break;
122+
case State::CLEAR_STATUS:
123+
if (time_in_state_ms() < 1) {
124+
return;
125+
}
126+
status.value = 0;
127+
status.command_status = 1; // cleared succesfully. Irony.
128+
set_state(State::IDLE);
129+
break;
130+
case State::MEASURING:
131+
if (time_in_state_ms() > 16) {
132+
measurement.temperature = get_sim_temperature();
133+
measurement.humidity = 33.3;
134+
set_state(State::MEASURED);
135+
}
136+
break;
137+
case State::MEASURED:
138+
break;
139+
case State::IDLE:
140+
break;
141+
}
142+
}
143+
144+
float SHT3x::get_sim_temperature() const
145+
{
146+
float sim_alt = AP::sitl()->state.altitude;
147+
sim_alt += 2 * rand_float();
148+
149+
// To Do: Add a sensor board temperature offset parameter
150+
return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25;
151+
}
152+
153+
#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED

‎libraries/SITL/SIM_Temperature_SHT3x.h‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
#include "SIM_config.h"
2+
3+
#if AP_SIM_TEMPERATURE_SHT3X_ENABLED
4+
5+
#include "SIM_I2CDevice.h"
6+
7+
/*
8+
Simulator for the SHT3X temperature sensor
9+
10+
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --speedup=1
11+
12+
param set TEMP1_TYPE 8
13+
param set TEMP1_ADDR 0x44
14+
15+
*/
16+
17+
namespace SITL {
18+
19+
class SHT3x : public I2CDevice
20+
{
21+
public:
22+
23+
SHT3x();
24+
25+
void update(const class Aircraft &aircraft) override;
26+
27+
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
28+
29+
private:
30+
31+
// should be a call on aircraft:
32+
float last_temperature = -1000.0f;
33+
34+
enum class State {
35+
UNKNOWN = 22,
36+
RESET = 23,
37+
CLEAR_STATUS = 24,
38+
IDLE = 25,
39+
MEASURING = 26,
40+
MEASURED = 27,
41+
} state = State::RESET;
42+
43+
uint32_t state_start_time_ms;
44+
45+
void set_state(State new_state) {
46+
state = new_state;
47+
state_start_time_ms = AP_HAL::millis();
48+
}
49+
uint32_t time_in_state_ms() const {
50+
return AP_HAL::millis() - state_start_time_ms;
51+
}
52+
53+
float get_sim_temperature() const;
54+
55+
float temperature_for_adc(uint32_t adc) const;
56+
uint32_t calculate_adc(float temperature) const;
57+
uint32_t adc;
58+
59+
void pack_reading(SITL::I2C::i2c_msg &msg);
60+
61+
enum class Command {
62+
RESET = 0x30A2,
63+
READ_STATUS = 0xF32D,
64+
CLEAR_STATUS = 0x3041,
65+
MEASURE = 0x2C06, // clock-stretching-enabled, high-repatability
66+
READ_SN = 0x3780,
67+
};
68+
69+
union {
70+
struct {
71+
uint8_t checksum_status : 1;
72+
uint8_t command_status : 1;
73+
uint8_t reserved1 : 2;
74+
uint8_t reset_detected : 1;
75+
uint8_t reserved2 : 5;
76+
uint8_t T_tracking_alert : 1;
77+
uint8_t RH_tracking_alert : 1;
78+
uint8_t reserved3 : 1;
79+
uint8_t header_status : 1;
80+
uint8_t reserved4 : 1;
81+
uint8_t alert_pending : 1;
82+
};
83+
uint16_t value;
84+
} status;
85+
86+
struct {
87+
float temperature;
88+
float humidity;
89+
} measurement;
90+
91+
void reset();
92+
};
93+
94+
} // namespace SITL
95+
96+
#endif // AP_SIM_TEMPERATURE_SHT3X_ENABLED

‎libraries/SITL/SIM_config.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,6 +192,10 @@
192192
#define AP_SIM_TEMPERATURE_MCP9600_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
193193
#endif
194194

195+
#ifndef AP_SIM_TEMPERATURE_SHT3X_ENABLED
196+
#define AP_SIM_TEMPERATURE_SHT3X_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
197+
#endif
198+
195199
#ifndef AP_SIM_TEMPERATURE_TSYS01_ENABLED
196200
#define AP_SIM_TEMPERATURE_TSYS01_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
197201
#endif

0 commit comments

Comments
 (0)