zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: add support for simulated TSYS01 temperature sensor

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
ecd21fbc4c
commit
8be50910e4
4 changed files with 257 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      libraries/SITL/SIM_I2C.cpp
  2. 1
      libraries/SITL/SIM_Submarine.cpp
  3. 189
      libraries/SITL/SIM_Temperature_TSYS01.cpp
  4. 64
      libraries/SITL/SIM_Temperature_TSYS01.h

3
libraries/SITL/SIM_I2C.cpp
Unescape View File

@ -26,6 +26,7 @@ @@ -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; @@ -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 { @@ -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 },
};

1
libraries/SITL/SIM_Submarine.cpp
Unescape View File

@ -235,6 +235,7 @@ void Submarine::update(const struct sitl_input &input) @@ -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
Unescape View File

@ -0,0 +1,189 @@ @@ -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
Unescape View File

@ -0,0 +1,64 @@ @@ -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
Write Preview
Loading…
Cancel
Save