SITL: add and use simulator for TOFSenseF I2C rangefinder

libraries/SITL/SIM_I2C.cpp

@@ -32,6 +32,7 @@
 #include "SIM_INA3221.h"
 #include "SIM_IS31FL3195.h"
 #include "SIM_RF_LightWareI2C_Legacy16Bit.h"
+#include "SIM_RF_TOFSenseF_I2C.h"
 #include "SIM_LM2755.h"
 #include "SIM_LP5562.h"
 #include "SIM_MaxSonarI2CXL.h"
@@ -135,6 +136,9 @@ static AS5600 as5600;  // AoA sensor
 static TFS20L tfs20l;  // Benewake TFS20L rangefinder
 #endif  // AP_SIM_TFS20L_ENABLED
 
+#if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+static TOFSenseF_I2C tofsensef;
+#endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -214,6 +218,9 @@ struct i2c_device_at_address {
 #if AP_SIM_TFS20L_ENABLED
     { 0, 0x10, tfs20l },          // RNGFNDx_TYPE = 46, RNGFNDx_ADDR = 0x10
 #endif  // AP_SIM_TFS20L_ENABLED
+#if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+    { 0, 0x08, tofsensef },          // RNGFNDx_TYPE = 40, RNGFNDx_ADDR = 0x08
+#endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED
 };
 
 void I2C::init()

libraries/SITL/SIM_RF_TOFSenseF_I2C.cpp

@@ -0,0 +1,88 @@
+#include "SIM_config.h"
+
+#if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+
+#include "SIM_RF_TOFSenseF_I2C.h"
+
+#include <stdio.h>
+
+#include <GCS_MAVLink/GCS.h>
+
+void SITL::TOFSenseF_I2C::update(const class Aircraft &aircraft)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    if (last_update_ms - now_ms < 50) {
+        return;
+    }
+    last_update_ms = now_ms;
+
+    range = aircraft.rangefinder_range();
+}
+
+int SITL::TOFSenseF_I2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    if (data->nmsgs != 1) {
+        abort();
+    }
+
+    switch (data->msgs[0].flags) {
+    case I2C_RDWR:
+        return SITL::TOFSenseF_I2C::take_reading(data);
+    case I2C_M_RD:
+        return SITL::TOFSenseF_I2C::read_data(data);
+    default:
+        break;
+    }
+    abort();
+}
+
+int SITL::TOFSenseF_I2C::take_reading(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    auto msg = data->msgs[0];
+    if (msg.flags != I2C_RDWR) {
+        abort();
+    }
+
+    if (msg.len != 2) {
+        abort();
+    }
+
+    if (msg.buf[0] != 0x24) {  // get distance
+        abort();
+    }
+
+    if (msg.buf[1] != 0x28) {  // get signal status
+        abort();
+    }
+
+    reading_requested = true;
+
+    return 0;
+}
+
+int SITL::TOFSenseF_I2C::read_data(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    if (!reading_requested) {
+        abort();
+    }
+
+    auto msg = data->msgs[0];
+
+    if (msg.flags != I2C_M_RD) {
+        abort();
+    }
+
+    if (msg.len != 8) {
+        abort();
+    }
+
+    put_le32_ptr(&msg.buf[0], uint32_t(range*1000));  // m -> mm
+
+    // 1 here means healthy, 2 here is current signal strength
+    const uint32_t signal_strength_and_status = (2U<<16 | 1);
+    put_le32_ptr(&msg.buf[4], signal_strength_and_status);
+
+    return 0;
+}
+
+#endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED

libraries/SITL/SIM_RF_TOFSenseF_I2C.h

@@ -0,0 +1,51 @@
+#pragma once
+
+/*
+  Simulator for the TOFSenseF I2C-connected rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1
+
+param set RNGFND1_TYPE 40
+# param set RNGFND1_ADDR 0x10
+param ftp
+param set RNGFND1_MAX 10  # ????
+graph DISTANCE_SENSOR[0].current_distance*0.01
+graph GLOBAL_POSITION_INT.relative_alt/1000-DISTANCE_SENSOR[0].current_distance*0.01
+reboot
+
+arm throttle
+rc 3 1600
+*/
+
+#include "SIM_config.h"
+
+#if AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class TOFSenseF_I2C : public I2CDevice
+{
+public:
+
+    using I2CDevice::I2CDevice;
+
+    void update(const class Aircraft &aircraft) override;
+
+private:
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+    int take_reading(I2C::i2c_rdwr_ioctl_data *&data);
+    int read_data(I2C::i2c_rdwr_ioctl_data *&data);
+
+    uint32_t last_update_ms;
+
+    float range;
+
+    bool reading_requested;
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED

libraries/SITL/SIM_config.h

@@ -249,6 +249,10 @@
 #define AP_SIM_RF_DTS6012M_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif  // AP_SIM_RF_DTS6012M_ENABLED
 
+#ifndef AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+#define AP_SIM_RF_TOFSENSEF_I2C_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif  // AP_SIM_RF_TOFSENSEF_I2C_ENABLED
+
 #ifndef AP_SIM_RAMTRON_ENABLED
 #define AP_SIM_RAMTRON_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif  // AP_SIM_RAMTRON_ENABLED