From dd2283d73b69e64d194affd0f214df4ac0ebea9d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 24 May 2022 14:14:25 +1000
Subject: [PATCH] AP_Airspeed: add SITL backend

---
 libraries/AP_Airspeed/AP_Airspeed.cpp      |  8 +++-
 libraries/AP_Airspeed/AP_Airspeed.h        |  1 +
 libraries/AP_Airspeed/AP_Airspeed_SITL.cpp | 45 ++++++++++++++++++++++
 libraries/AP_Airspeed/AP_Airspeed_SITL.h   | 36 +++++++++++++++++
 4 files changed, 89 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_Airspeed/AP_Airspeed_SITL.cpp
 create mode 100644 libraries/AP_Airspeed/AP_Airspeed_SITL.h

diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp
index 8752ddbfbe..2241b2e856 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed.cpp
@@ -48,6 +48,7 @@
 #include "AP_Airspeed_UAVCAN.h"
 #include "AP_Airspeed_NMEA.h"
 #include "AP_Airspeed_MSP.h"
+#include "AP_Airspeed_SITL.h"
 extern const AP_HAL::HAL &hal;
 
 #ifdef HAL_AIRSPEED_TYPE_DEFAULT
@@ -94,7 +95,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),     // NOTE: Index 0 is actually used as index 63 here
 
@@ -390,6 +391,11 @@ void AP_Airspeed::init()
         case TYPE_I2C_MS4525:
 #if AP_AIRSPEED_MS4525_ENABLED
             sensor[i] = new AP_Airspeed_MS4525(*this, i);
+#endif
+            break;
+        case TYPE_SITL:
+#if AP_AIRSPEED_SITL_ENABLED
+            sensor[i] = new AP_Airspeed_SITL(*this, i);
 #endif
             break;
         case TYPE_ANALOG:
diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h
index b8015cb670..b3983d6515 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.h
+++ b/libraries/AP_Airspeed/AP_Airspeed.h
@@ -156,6 +156,7 @@ public:
         TYPE_NMEA_WATER=13,
         TYPE_MSP=14,
         TYPE_I2C_ASP5033=15,
+        TYPE_SITL=100,
     };
 
     // get current primary sensor
diff --git a/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp b/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp
new file mode 100644
index 0000000000..92ab4357cc
--- /dev/null
+++ b/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp
@@ -0,0 +1,45 @@
+#include "AP_Airspeed_SITL.h"
+
+#if AP_AIRSPEED_SITL_ENABLED
+
+#include <AP_Baro/AP_Baro.h>
+#include <SITL/SITL.h>
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_SITL::get_differential_pressure(float &pressure)
+{
+    const uint8_t _instance = get_instance();
+
+    if (_instance >= AIRSPEED_MAX_SENSORS) {
+        return false;
+    }
+
+    pressure = AP::sitl()->state.airspeed_raw_pressure[_instance];
+
+    return true;
+}
+
+// get last temperature
+bool AP_Airspeed_SITL::get_temperature(float &temperature)
+{
+    const uint8_t _instance = get_instance();
+
+    if (_instance >= AIRSPEED_MAX_SENSORS) {
+        return false;
+    }
+
+    const auto *sitl = AP::sitl();
+
+    // this was mostly swiped from SIM_Airspeed_DLVR:
+    const float sim_alt = sitl->state.altitude;
+
+    float sigma, delta, theta;
+    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
+
+    // To Do: Add a sensor board temperature offset parameter
+    temperature = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
+
+    return true;
+}
+
+#endif // AP_AIRSPEED_SITL_ENABLED
diff --git a/libraries/AP_Airspeed/AP_Airspeed_SITL.h b/libraries/AP_Airspeed/AP_Airspeed_SITL.h
new file mode 100644
index 0000000000..64f6da2999
--- /dev/null
+++ b/libraries/AP_Airspeed/AP_Airspeed_SITL.h
@@ -0,0 +1,36 @@
+/*
+  SITL airspeed backend - a perfect airspeed sensor
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#ifndef AP_AIRSPEED_SITL_ENABLED
+#define AP_AIRSPEED_SITL_ENABLED AP_SIM_ENABLED
+#endif
+
+#if AP_AIRSPEED_SITL_ENABLED
+
+#include "AP_Airspeed_Backend.h"
+
+class AP_Airspeed_SITL : public AP_Airspeed_Backend
+{
+public:
+
+    using AP_Airspeed_Backend::AP_Airspeed_Backend;
+
+    bool init(void) override {
+        return true;
+    }
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure) override;
+
+    // temperature not available via analog backend
+    bool get_temperature(float &temperature) override;
+
+private:
+};
+
+#endif // AP_AIRSPEED_SITL_ENABLED