From 6bc8ff2ee848486bdd859e66142e3117b2dee011 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 4 Dec 2020 14:14:59 +1100
Subject: [PATCH] SITL: factor out an I2C command/response class from simulated
 MaxSonar sensor

---
 libraries/SITL/SIM_I2CDevice.cpp     | 34 +++++++++++++++++++++
 libraries/SITL/SIM_I2CDevice.h       | 19 ++++++++++++
 libraries/SITL/SIM_MaxSonarI2CXL.cpp | 45 ----------------------------
 libraries/SITL/SIM_MaxSonarI2CXL.h   | 18 ++++++++---
 4 files changed, 67 insertions(+), 49 deletions(-)
 delete mode 100644 libraries/SITL/SIM_MaxSonarI2CXL.cpp

diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp
index 54a2bc6a4a..0ab7554974 100644
--- a/libraries/SITL/SIM_I2CDevice.cpp
+++ b/libraries/SITL/SIM_I2CDevice.cpp
@@ -125,3 +125,37 @@ int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
 
     return -1;
 };
+
+
+int SITL::I2CCommandResponseDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    const uint32_t now = AP_HAL::millis();
+
+    struct I2C::i2c_msg &msg = data->msgs[0];
+    if (msg.flags == I2C_M_RD) {
+        // driver is attempting to receive reading...
+        if (now - cmd_take_reading_received_ms < command_processing_time_ms()) {
+            // not sure we ought to be returning -1 here - what does
+            // the real device do?  return stale data?  garbage data?
+            return -1;
+        }
+        if (msg.len != 2) {
+            AP_HAL::panic("Unxpected message length (%u)", msg.len);
+        }
+
+        const uint16_t value = reading();
+
+        msg.buf[0] = value >> 8;
+        msg.buf[1] = value & 0xff;
+
+        return 0;
+    }
+
+    const uint8_t cmd = msg.buf[0];
+    if (cmd != command_take_reading()) {
+        AP_HAL::panic("Unknown command (%u)", cmd);
+    }
+    cmd_take_reading_received_ms = now;
+
+    return 0;
+}
diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h
index 7112bda90c..d7396b06a8 100644
--- a/libraries/SITL/SIM_I2CDevice.h
+++ b/libraries/SITL/SIM_I2CDevice.h
@@ -50,6 +50,25 @@ protected:
     uint8_t byte[256];
 };
 
+/*
+  for devices that take a command then will provide data to a read();
+  for example, MCU writes 0x07 to device then expects to be able to
+  read 2 bytes from it.
+*/
+class I2CCommandResponseDevice {
+public:
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data);
+
+protected:
+
+    // time taken for device to process command:
+    uint16_t command_processing_time_ms() const { return 20; }
+    virtual uint8_t command_take_reading() const = 0;
+    virtual uint16_t reading() const = 0;
+
+    uint32_t cmd_take_reading_received_ms;
+};
+
 class I2CDevice {
 public:
     virtual void init() {}
diff --git a/libraries/SITL/SIM_MaxSonarI2CXL.cpp b/libraries/SITL/SIM_MaxSonarI2CXL.cpp
deleted file mode 100644
index 22754afd2f..0000000000
--- a/libraries/SITL/SIM_MaxSonarI2CXL.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "SIM_MaxSonarI2CXL.h"
-
-#include <SITL/SITL.h>
-#include <SITL/SIM_Aircraft.h>
-
-#include <stdio.h>
-
-int SITL::MaxSonarI2CXL::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
-{
-    const uint32_t now = AP_HAL::millis();
-
-    struct I2C::i2c_msg &msg = data->msgs[0];
-    if (msg.flags == I2C_M_RD) {
-        // driver is attempting to receive reading...
-        if (now - cmd_take_reading_received_ms < 20) {
-            // not sure we ought to be returning -1 here - what does
-            // the real device do?  return stale data?  garbage data?
-            return -1;
-        }
-        if (msg.len != 2) {
-            AP_HAL::panic("Unxpected message length (%u)", msg.len);
-        }
-
-        const uint16_t range_cm = rangefinder_range * 100;
-
-        msg.buf[0] = range_cm >> 8;
-        msg.buf[1] = range_cm & 0xff;
-
-        return 0;
-    }
-
-    const uint8_t CMD_TAKE_READING  = 0x51;
-    const uint8_t cmd = msg.buf[0];
-    if (cmd != CMD_TAKE_READING) {
-        AP_HAL::panic("Unknown command (%u)", cmd);
-    }
-    cmd_take_reading_received_ms = now;
-
-    return 0;
-};
-
-void SITL::MaxSonarI2CXL::update(const Aircraft &aircraft)
-{
-    rangefinder_range = aircraft.rangefinder_range();
-}
diff --git a/libraries/SITL/SIM_MaxSonarI2CXL.h b/libraries/SITL/SIM_MaxSonarI2CXL.h
index 5abef50dd7..a18d981745 100644
--- a/libraries/SITL/SIM_MaxSonarI2CXL.h
+++ b/libraries/SITL/SIM_MaxSonarI2CXL.h
@@ -4,16 +4,26 @@
 
 namespace SITL {
 
-class MaxSonarI2CXL : public I2CDevice
+class MaxSonarI2CXL : public I2CDevice, public I2CCommandResponseDevice
 {
 public:
 
-    void update(const class Aircraft &aircraft) override;
+    // methods to make I2CCommandResponseDevice happy:
+    uint8_t command_take_reading() const override { return 0x51; }
+    uint16_t reading() const override {
+        return rangefinder_range * 100;
+    };
+
+    void update(const class Aircraft &aircraft) override {
+        rangefinder_range = aircraft.rangefinder_range();
+    }
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
+        return I2CCommandResponseDevice::rdwr(data);
+    }
 
-    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
 
 private:
-    uint32_t cmd_take_reading_received_ms;
 
     float rangefinder_range;
 };