mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-20 14:54:09 -04:00
SITL: add support for simulated TSYS01 temperature sensor
This commit is contained in:
Peter Barker
Peter Barker
parent
ecd21fbc4c
commit
8be50910e4
@ -26,6 +26,7 @@
|
||||
#include "SIM_BattMonitor_SMBus_Maxell.h"
|
||||
#include "SIM_BattMonitor_SMBus_Rotoye.h"
|
||||
#include "SIM_Airspeed_DLVR.h"
|
||||
#include "SIM_Temperature_TSYS01.h"
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
@ -49,6 +50,7 @@ static MaxSonarI2CXL maxsonari2cxl;
|
||||
static Maxell maxell;
|
||||
static Rotoye rotoye;
|
||||
static Airspeed_DLVR airspeed_dlvr;
|
||||
static TSYS01 tsys01;
|
||||
|
||||
struct i2c_device_at_address {
|
||||
uint8_t bus;
|
||||
@ -61,6 +63,7 @@ struct i2c_device_at_address {
|
||||
{ 1, 0x39, ignored }, // NCP5623C
|
||||
{ 1, 0x40, ignored }, // KellerLD
|
||||
{ 1, 0x76, ignored }, // MS56XX
|
||||
{ 1, 0x77, tsys01 },
|
||||
{ 1, 0x0B, rotoye },
|
||||
{ 2, 0x28, airspeed_dlvr },
|
||||
};
|
||||
|
||||
@ -235,6 +235,7 @@ void Submarine::update(const struct sitl_input &input)
|
||||
calculate_forces(input, rot_accel, accel_body);
|
||||
|
||||
update_dynamics(rot_accel);
|
||||
update_external_payload(input);
|
||||
|
||||
// update lat/lon/altitude
|
||||
update_position();
|
||||
|
||||
189
libraries/SITL/SIM_Temperature_TSYS01.cpp
Normal file
189
libraries/SITL/SIM_Temperature_TSYS01.cpp
Normal file
@ -0,0 +1,189 @@
|
||||
#include "SIM_Temperature_TSYS01.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
constexpr const int32_t SITL::TSYS01::_k[5];
|
||||
|
||||
int SITL::TSYS01::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 uint8_t command = data->msgs[0].buf[0];
|
||||
switch ((Command)command) {
|
||||
case Command::RESET:
|
||||
AP_HAL::panic("Bad RESET");
|
||||
case Command::READ_PROM0:
|
||||
case Command::READ_PROM1:
|
||||
case Command::READ_PROM2:
|
||||
case Command::READ_PROM3:
|
||||
case Command::READ_PROM4:
|
||||
case Command::READ_PROM5: {
|
||||
if (state != State::RESET) {
|
||||
AP_HAL::panic("reading prom outside RESET state");
|
||||
}
|
||||
if (data->msgs[1].len != 2) {
|
||||
AP_HAL::panic("Unexpected prom read length");
|
||||
}
|
||||
uint8_t offs = 5-((uint8_t(command) - uint8_t(Command::READ_PROM0))/2);
|
||||
const uint16_t k = _k[offs];
|
||||
data->msgs[1].buf[0] = k >> 8;
|
||||
data->msgs[1].buf[1] = k & 0xFF;
|
||||
break;
|
||||
}
|
||||
case Command::CONVERT:
|
||||
AP_HAL::panic("Bad CONVERT");
|
||||
case Command::READ_ADC: {
|
||||
uint8_t registers[3] {};
|
||||
if (data->msgs[1].len != sizeof(registers)) {
|
||||
AP_HAL::panic("Unexpected prom read length");
|
||||
}
|
||||
if (state == State::CONVERTING) {
|
||||
// we've been asked for values while still converting.
|
||||
// Return zeroes per data sheet
|
||||
} else if (state == State::CONVERTED) {
|
||||
uint32_t value = adc;
|
||||
registers[2] = value & 0xff;
|
||||
value >>= 8;
|
||||
registers[1] = value & 0xff;
|
||||
value >>= 8;
|
||||
registers[0] = value & 0xff;
|
||||
set_state(State::IDLE);
|
||||
} else {
|
||||
// AP_HAL::panic("READ_ADC in bad state");
|
||||
// this happens at startup
|
||||
return -1;
|
||||
}
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(registers); i++) {
|
||||
data->msgs[1].buf[i] = registers[i];
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (data->nmsgs == 1) {
|
||||
// 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_PROM0:
|
||||
case Command::READ_PROM1:
|
||||
case Command::READ_PROM2:
|
||||
case Command::READ_PROM3:
|
||||
case Command::READ_PROM4:
|
||||
case Command::READ_PROM5:
|
||||
AP_HAL::panic("bad prom read");
|
||||
case Command::CONVERT:
|
||||
if (state != State::RESET &&
|
||||
state != State::CONVERTING &&
|
||||
state != State::IDLE &&
|
||||
state != State::READ_PROM) {
|
||||
AP_HAL::panic("Convert outside reset/idle");
|
||||
}
|
||||
set_state(State::CONVERTING);
|
||||
break;
|
||||
case Command::READ_ADC:
|
||||
AP_HAL::panic("bad READ_ADC");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
// swiped from the driver:
|
||||
float SITL::TSYS01::temperature_for_adc(uint32_t _adc) const
|
||||
{
|
||||
const float adc16 = _adc/256.0;
|
||||
// const uint32_t _k[] { 28446, 24926, 36016, 32791, 40781 };
|
||||
return
|
||||
-2 * _k[4] * powf(10, -21) * powf(adc16, 4) +
|
||||
4 * _k[3] * powf(10, -16) * powf(adc16, 3) +
|
||||
-2 * _k[2] * powf(10, -11) * powf(adc16, 2) +
|
||||
1 * _k[1] * powf(10, -6) * adc16 +
|
||||
-1.5 * _k[0] * powf(10, -2);
|
||||
}
|
||||
|
||||
uint32_t SITL::TSYS01::calculate_adc(float temperature) const
|
||||
{
|
||||
// bisect to find the adc24 value:
|
||||
uint32_t min_adc = 0;
|
||||
uint32_t max_adc = 1<<24;
|
||||
uint32_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(max_adc - min_adc) > 1 && current_error > 0.05) {
|
||||
uint32_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::TSYS01::update(const class Aircraft &aircraft)
|
||||
{
|
||||
switch (state) {
|
||||
case State::UNKNOWN:
|
||||
break;
|
||||
case State::RESET:
|
||||
if (time_in_state_ms() > 10) {
|
||||
set_state(State::READ_PROM);
|
||||
}
|
||||
break;
|
||||
case State::READ_PROM:
|
||||
break;
|
||||
case State::IDLE:
|
||||
break;
|
||||
case State::CONVERTING:
|
||||
if (time_in_state_ms() > 5) {
|
||||
if (!is_equal(last_temperature, some_temperature)) {
|
||||
last_temperature = some_temperature;
|
||||
adc = calculate_adc(some_temperature);
|
||||
}
|
||||
set_state(State::CONVERTED);
|
||||
}
|
||||
break;
|
||||
case State::CONVERTED:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
64
libraries/SITL/SIM_Temperature_TSYS01.h
Normal file
64
libraries/SITL/SIM_Temperature_TSYS01.h
Normal file
@ -0,0 +1,64 @@
|
||||
#include "SIM_I2CDevice.h"
|
||||
|
||||
/*
|
||||
Simulator for the TSYS01 temperature sensor
|
||||
|
||||
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --speedup=1
|
||||
|
||||
*/
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class TSYS01 : public I2CDevice
|
||||
{
|
||||
public:
|
||||
|
||||
void update(const class Aircraft &aircraft) override;
|
||||
|
||||
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
|
||||
|
||||
private:
|
||||
|
||||
// should be a call on aircraft:
|
||||
float some_temperature = 26.5;
|
||||
float last_temperature = -1000.0f;
|
||||
|
||||
enum class State {
|
||||
UNKNOWN = 22,
|
||||
RESET = 23,
|
||||
READ_PROM = 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 temperature_for_adc(uint32_t adc) const;
|
||||
uint32_t calculate_adc(float temperature) const;
|
||||
uint32_t adc;
|
||||
|
||||
enum class Command {
|
||||
RESET = 0x1E,
|
||||
READ_PROM0 = 0xA0,
|
||||
READ_PROM1 = 0xA2,
|
||||
READ_PROM2 = 0xA4,
|
||||
READ_PROM3 = 0xA6,
|
||||
READ_PROM4 = 0xA8,
|
||||
READ_PROM5 = 0xAA,
|
||||
CONVERT = 0x40,
|
||||
READ_ADC = 0x00,
|
||||
};
|
||||
|
||||
static constexpr int32_t _k[] { 40781, 32791, 36016, 24926, 28446 };
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
Loading…
Reference in New Issue
Block a user