diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index c1e3468f6e7..8d0bfd55bfa 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -38,6 +38,7 @@ #include "SIM_Temperature_MCP9600.h" #include "SIM_Temperature_TSYS01.h" #include "SIM_Temperature_TSYS03.h" +#include "SIM_TeraRangerI2C.h" #include "SIM_ToshibaLED.h" #include @@ -108,12 +109,18 @@ static QMC5883L qmc5883l; #if AP_SIM_INA3221_ENABLED static INA3221 ina3221; #endif +#if AP_SIM_TERARANGERI2C_ENABLED +static TeraRangerI2C terarangeri2c; +#endif struct i2c_device_at_address { uint8_t bus; uint8_t addr; I2CDevice &device; } i2c_devices[] { +#if AP_SIM_TERARANGERI2C_ENABLED + { 0, 0x31, terarangeri2c }, // RNGFNDx_TYPE = 14, RNGFNDx_ADDR = 49 +#endif // AP_SIM_TERARANGERI2C_ENABLED #if AP_SIM_MAXSONAR_I2C_XL_ENABLED { 0, 0x70, maxsonari2cxl }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 112 #endif diff --git a/libraries/SITL/SIM_TeraRangerI2C.cpp b/libraries/SITL/SIM_TeraRangerI2C.cpp new file mode 100644 index 00000000000..abe63a74c16 --- /dev/null +++ b/libraries/SITL/SIM_TeraRangerI2C.cpp @@ -0,0 +1,55 @@ +#include "SIM_config.h" + +#if AP_SIM_TERARANGERI2C_ENABLED + +#include "SIM_TeraRangerI2C.h" + +using namespace SITL; + +int TeraRangerI2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + // read/write message + const uint8_t reg_base_addr = data->msgs[0].buf[0]; + if (reg_base_addr == 0) { + AP_HAL::panic("Should not get combed read/write for TRIGGER_READING"); + } + } + + if (data->msgs[0].flags == I2C_M_RD) { + if (reading_start_us != 0) { + if (data->nmsgs != 1) { + AP_HAL::panic("Unexpected number of i2c messages"); + } + const auto &msg = data->msgs[0]; + if (reading_start_us == 0) { + AP_HAL::panic("Attempt to read sample without requesting it"); + } + const uint32_t now_us = MAX(AP_HAL::micros(), 1U); + if (now_us - reading_start_us < 500) { + AP_HAL::panic("Attempt to read sensor before data would be ready"); + } + const uint16_t reading = MIN(rangefinder_range*1000, 65535); + msg.buf[0] = reading >> 8; + msg.buf[1] = reading & 0xff; + msg.buf[2] = crc_crc8(msg.buf, 2); + reading_start_us = 0; + return 0; + } + } else if (data->msgs[0].flags == 0) { // write request + const uint8_t reg_base_addr = data->msgs[0].buf[0]; + if (reg_base_addr == 0) { + if (reading_start_us != 0) { + AP_HAL::panic("Requesting sample without reading previous one?"); + } + reading_start_us = MAX(AP_HAL::micros(), 1U); + return 0; + } + } else { + AP_HAL::panic("Bad flags in i2c transfer %02x", data->msgs[0].flags); + } + + return I2CRegisters_8Bit::rdwr(data); +} + +#endif // AP_SIM_TERARANGERI2C_ENABLED diff --git a/libraries/SITL/SIM_TeraRangerI2C.h b/libraries/SITL/SIM_TeraRangerI2C.h new file mode 100644 index 00000000000..406fa40acc8 --- /dev/null +++ b/libraries/SITL/SIM_TeraRangerI2C.h @@ -0,0 +1,62 @@ +#pragma once + +/* + Simulator for the TeraRangerI2C rangefinder + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1 + +param set RNGFND1_TYPE 14 # TRI2C +param ftp +param set RNGFND1_ADDR 49 +graph RANGEFINDER.distance +graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance +reboot + +arm throttle +rc 3 1600 +*/ + +#include "SIM_config.h" + +#if AP_SIM_TERARANGERI2C_ENABLED + +#include "SIM_I2CDevice.h" + +namespace SITL { + +class TeraRangerI2CDevReg : public I2CRegEnum { +public: + // static constexpr uint8_t TRIGGER_READING = 0x00; + static constexpr uint8_t WHO_AM_I = 0x01; + static constexpr uint8_t CHANGE_BASE_ADDR = 0xA2; +}; + +class TeraRangerI2C : public I2CDevice, public I2CRegisters_8Bit +{ +public: + + void init() override { + // add_register("TRIGGER_READING", TeraRangerI2CDevReg::TRIGGER_READING, I2CRegisters::RegMode::WRONLY); + add_register("WHO_AM_I", TeraRangerI2CDevReg::WHO_AM_I, I2CRegisters::RegMode::RDONLY); + add_register("CHANGE_BASE_ADDR", TeraRangerI2CDevReg::CHANGE_BASE_ADDR, I2CRegisters::RegMode::WRONLY); + + set_register(TeraRangerI2CDevReg::WHO_AM_I, (uint8_t)0xA1); + } + + void update(const class Aircraft &aircraft) override { + // free running + rangefinder_range = aircraft.rangefinder_range(); + } + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + +private: + + float rangefinder_range; + + uint32_t reading_start_us; +}; + +} // namespace SITL + +#endif // AP_SIM_TERARANGERI2C_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 3b9eca53c98..74dba060cee 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -65,6 +65,10 @@ #define AP_SIM_TETHER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_TERARANGERI2C_ENABLED +#define AP_SIM_TERARANGERI2C_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_FLIGHTAXIS_ENABLED #define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif