add teraranger simulator

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 <signal.h>
@@ -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

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

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

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