SITL: add simulated DLVR airspeed sensor

Tools/autotest/ArduPlane_Tests/AirspeedDrivers/ap1.txt

@@ -0,0 +1,9 @@
+QGC WPL 110
+0	1	0	16	0	0	0	0	-35.362881	149.165222	582.000000	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	100.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	100.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361721	149.161835	40.000000	1
+4	0	3	178	0.000000	13.00000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367970	149.164124	28.000000	1
+6	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366814	149.165878	28.000000	1
+7	0	3	21	0.000000	0.000000	0.000000	0.000000	-35.362911	149.165222	0.000000	1

Tools/autotest/arduplane.py

@@ -1853,6 +1853,13 @@ def fly_soaring(self):
 
         self.progress("Mission OK")
 
+    def test_airspeed_drivers(self):
+        self.set_parameter("ARSPD2_TYPE", 7)
+        self.reboot_sitl()
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.fly_mission("ap1.txt")
+
     def fly_terrain_mission(self):
 
         self.customise_SITL_commandline([], wipe=True)
@@ -2157,6 +2164,10 @@ def tests(self):
              "Test EKF3 Affinity and Lane Switching",
              self.ekf_lane_switch),
 
+            ("AirspeedDrivers",
+             "Test AirSpeed drivers",
+             self.test_airspeed_drivers),
+
             ("LogUpload",
              "Log upload",
              self.log_upload),

libraries/AP_HAL_SITL/SITL_State.h

@@ -175,9 +175,6 @@ class HALSITL::SITL_State {
     void _fdm_input_local(void);
     void _output_to_flightgear(void);
     void _simulator_servos(struct sitl_input &input);
-    void _simulator_output(bool synthetic_clock_mode);
-    uint16_t _airspeed_sensor(float airspeed);
-    uint16_t _ground_sonar();
     void _fdm_input_step(void);
 
     void wait_clock(uint64_t wait_time_usec);

libraries/AP_HAL_SITL/sitl_airspeed.cpp

@@ -66,6 +66,9 @@ void SITL_State::_update_airspeed(float airspeed)
     float airspeed_raw = airspeed_pressure + _sitl->arspd_offset[0];
     float airspeed2_raw = airspeed2_pressure + _sitl->arspd_offset[1];
 
+    _sitl->state.airspeed_raw_pressure[0] = airspeed_pressure;
+    _sitl->state.airspeed_raw_pressure[1] = airspeed2_pressure;
+
     if (airspeed_raw / 4 > 0xFFFF) {
         airspeed_pin_value = 0xFFFF;
         return;

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;
+}

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

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()

libraries/SITL/SITL.h

@@ -74,6 +74,7 @@ struct sitl_fdm {
     } scanner;
 
     float rangefinder_m[RANGEFINDER_MAX_INSTANCES];
+    float airspeed_raw_pressure[2];
 
     struct {
         float speed;