From 0bb6f8897e0c058932363ed4824a4129bc0bf83b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 11 Dec 2020 11:21:49 +1100
Subject: [PATCH] SITL: add simulated DLVR airspeed sensor

---
 libraries/SITL/SIM_Airspeed_DLVR.cpp | 65 ++++++++++++++++++++++++++++
 libraries/SITL/SIM_Airspeed_DLVR.h   | 28 ++++++++++++
 libraries/SITL/SIM_I2C.cpp           |  3 ++
 3 files changed, 96 insertions(+)
 create mode 100644 libraries/SITL/SIM_Airspeed_DLVR.cpp
 create mode 100644 libraries/SITL/SIM_Airspeed_DLVR.h

diff --git a/libraries/SITL/SIM_Airspeed_DLVR.cpp b/libraries/SITL/SIM_Airspeed_DLVR.cpp
new file mode 100644
index 0000000000..f1f6a2e16d
--- /dev/null
+++ b/libraries/SITL/SIM_Airspeed_DLVR.cpp
@@ -0,0 +1,65 @@
+#include "SIM_Airspeed_DLVR.h"
+
+#include "SITL.h"
+
+int SITL::Airspeed_DLVR::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    struct I2C::i2c_msg &msg = data->msgs[0];
+    if (msg.flags == I2C_M_RD) {
+        // driver is attempting to receive reading...
+        if (msg.len != 4) {
+            AP_HAL::panic("Unexpected message length (%u)", msg.len);
+        }
+
+        uint8_t status = 0;
+        if (last_sent_ms == last_update_ms) {
+            status |= 0b10;
+        }
+        last_sent_ms = last_update_ms;
+        // if (electrical_fault_or_bad_config) {
+        //     status |= 0b11;
+        // }
+
+        // calculation of packed-pressure value:
+
+        // this is TYPE_I2C_DLVR_5IN
+        const uint8_t range_inH2O = 5;
+
+#define DLVR_OFFSET 8192.0f
+#define DLVR_SCALE 16384.0f
+
+//        const float pressure_pascals = 1555.2f;  // maximum transportable
+        const float press_h2o = pressure * (1.0f/INCH_OF_H2O_TO_PASCAL);
+        const uint32_t pressure_raw = ((press_h2o / (1.25f * 2.0f * range_inH2O)) * DLVR_SCALE) + DLVR_OFFSET;
+
+        // calculation of packed-temperature value
+        const uint32_t temp_raw = (temperature + 50.0f)  * (2047.0f/200.0f);
+
+        const uint32_t packed =
+            (status << 30) |
+            ((pressure_raw & 0x3fff) << 16) |
+            ((temp_raw & 0x7ff) << 5);
+
+        msg.buf[0] = (packed >> 24) & 0xff;
+        msg.buf[1] = (packed >> 16) & 0xff;
+        msg.buf[2] = (packed >>  8) & 0xff;
+        msg.buf[3] = (packed >>  0) & 0xff;
+
+        return 0;
+    }
+
+    AP_HAL::panic("Should never be written to");
+}
+
+
+void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_update_ms < 50) { // 20Hz
+        return;
+    }
+    last_update_ms = now_ms;
+
+    pressure = AP::sitl()->state.airspeed_raw_pressure[0];
+    temperature = 25.0f;
+}
diff --git a/libraries/SITL/SIM_Airspeed_DLVR.h b/libraries/SITL/SIM_Airspeed_DLVR.h
new file mode 100644
index 0000000000..cb21efdb51
--- /dev/null
+++ b/libraries/SITL/SIM_Airspeed_DLVR.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class Airspeed_DLVR : public I2CDevice
+{
+public:
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+
+    void update(const class Aircraft &aircraft) override;
+
+private:
+
+    float pressure;
+    float temperature;
+
+    // time we last updated the measurements (simulated internal
+    // workings of sensor)
+    uint32_t last_update_ms;
+
+    // time we last responded to an i2c request for data:
+    uint32_t last_sent_ms;
+};
+
+} // namespace SITL
diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp
index fe22ea396c..1e2a1f6eec 100644
--- a/libraries/SITL/SIM_I2C.cpp
+++ b/libraries/SITL/SIM_I2C.cpp
@@ -25,6 +25,7 @@
 #include "SIM_MaxSonarI2CXL.h"
 #include "SIM_BattMonitor_SMBus_Maxell.h"
 #include "SIM_BattMonitor_SMBus_Rotoye.h"
+#include "SIM_Airspeed_DLVR.h"
 
 #include <signal.h>
 
@@ -47,6 +48,7 @@ static ToshibaLED toshibaled;
 static MaxSonarI2CXL maxsonari2cxl;
 static Maxell maxell;
 static Rotoye rotoye;
+static Airspeed_DLVR airspeed_dlvr;
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -60,6 +62,7 @@ struct i2c_device_at_address {
     { 1, 0x70, maxsonari2cxl },
     { 1, 0x76, ignored }, // MS56XX
     { 2, 0x0B, rotoye },
+    { 2, 0x28, airspeed_dlvr },
 };
 
 void I2C::init()