From b5050c1e11cc99c2b8c66026228e4d01207e2a15 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 3 Aug 2020 13:23:10 +1000
Subject: [PATCH] SITL: add simulated MaxSonarI2CXL i2c sensor

---
 libraries/SITL/SIM_I2C.cpp           |  3 ++
 libraries/SITL/SIM_MaxSonarI2CXL.cpp | 45 ++++++++++++++++++++++++++++
 libraries/SITL/SIM_MaxSonarI2CXL.h   | 21 +++++++++++++
 3 files changed, 69 insertions(+)
 create mode 100644 libraries/SITL/SIM_MaxSonarI2CXL.cpp
 create mode 100644 libraries/SITL/SIM_MaxSonarI2CXL.h

diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp
index 6ebc687e35..d3130640fd 100644
--- a/libraries/SITL/SIM_I2C.cpp
+++ b/libraries/SITL/SIM_I2C.cpp
@@ -22,6 +22,7 @@
 
 #include "SIM_I2C.h"
 #include "SIM_ToshibaLED.h"
+#include "SIM_MaxSonarI2CXL.h"
 
 #include <signal.h>
 
@@ -41,6 +42,7 @@ public:
 static IgnoredI2CDevice ignored;
 
 static ToshibaLED toshibaled;
+static MaxSonarI2CXL maxsonari2cxl;
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -51,6 +53,7 @@ struct i2c_device_at_address {
     { 1, 0x38, ignored }, // NCP5623
     { 1, 0x39, ignored }, // NCP5623C
     { 1, 0x40, ignored }, // KellerLD
+    { 1, 0x70, maxsonari2cxl },
     { 1, 0x76, ignored }, // MS56XX
 };
 
diff --git a/libraries/SITL/SIM_MaxSonarI2CXL.cpp b/libraries/SITL/SIM_MaxSonarI2CXL.cpp
new file mode 100644
index 0000000000..22754afd2f
--- /dev/null
+++ b/libraries/SITL/SIM_MaxSonarI2CXL.cpp
@@ -0,0 +1,45 @@
+#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
new file mode 100644
index 0000000000..5abef50dd7
--- /dev/null
+++ b/libraries/SITL/SIM_MaxSonarI2CXL.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class MaxSonarI2CXL : public I2CDevice
+{
+public:
+
+    void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+
+private:
+    uint32_t cmd_take_reading_received_ms;
+
+    float rangefinder_range;
+};
+
+} // namespace SITL