mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-13 10:03:57 -03:00
SITL: add simulated SHT3x temperature sensor
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
0ae3e7cdcb
commit
8df29a3960
@ -21,11 +21,12 @@
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
#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
|
||||
|
||||
153
libraries/SITL/SIM_Temperature_SHT3x.cpp
Normal file
153
libraries/SITL/SIM_Temperature_SHT3x.cpp
Normal file
@ -0,0 +1,153 @@
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_TEMPERATURE_SHT3X_ENABLED
|
||||
|
||||
#include "SIM_Temperature_SHT3x.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
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
|
||||
96
libraries/SITL/SIM_Temperature_SHT3x.h
Normal file
96
libraries/SITL/SIM_Temperature_SHT3x.h
Normal file
@ -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
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user