From d0ff2089c4e4ddd27a112573809a202c34a34ad0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Oct 2019 21:02:56 +1000
Subject: [PATCH] AP_Periph: enable airspeed sensor

---
 Tools/AP_Periph/AP_Periph.cpp  |  4 +++
 Tools/AP_Periph/AP_Periph.h    |  7 ++++
 Tools/AP_Periph/Parameters.cpp |  7 ++++
 Tools/AP_Periph/Parameters.h   |  1 +
 Tools/AP_Periph/can.cpp        | 63 ++++++++++++++++++++++++++++++++++
 Tools/AP_Periph/wscript        |  1 +
 6 files changed, 83 insertions(+)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 0518d52b6c..2efa677779 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -79,6 +79,10 @@ void AP_Periph_FW::init()
     adsb_init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    airspeed.init();
+#endif
+    
     start_ms = AP_HAL::millis();
 }
 
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 8032ab846e..39a92fd048 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -3,6 +3,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_Airspeed/AP_Airspeed.h>
 
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED)
 #define AP_PERIPH_HAVE_LED
@@ -24,6 +25,7 @@ public:
     void can_mag_update();
     void can_gps_update();
     void can_baro_update();
+    void can_airspeed_update();
 
     void load_parameters();
 
@@ -50,6 +52,10 @@ public:
         mavlink_status_t status;
     } adsb;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    AP_Airspeed airspeed;
+#endif
     
     // setup the var_info table
     AP_Param param_loader{var_info};
@@ -59,6 +65,7 @@ public:
     uint32_t last_mag_update_ms;
     uint32_t last_gps_update_ms;
     uint32_t last_baro_update_ms;
+    uint32_t last_airspeed_update_ms;
 };
 
 extern AP_Periph_FW periph;
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index e14e2024d9..c464670084 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -58,6 +58,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(led_brightness, "LED_BRIGHTNESS", AP_PERIPH_LED_BRIGHT_DEFAULT),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    // Airspeed driver
+    // @Group: ARSP
+    // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
+    GOBJECT(airspeed, "ARSP", AP_Airspeed),
+#endif
+    
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 8ebc51ee0d..5bfe14f7a9 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -19,6 +19,7 @@ public:
         k_param_baro,
         k_param_buzz_volume,
         k_param_led_brightness,
+        k_param_airspeed,
     };
 
     AP_Int16 format_version;
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index f35a3ac87a..9d4cae452b 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -32,6 +32,7 @@
 #include <uavcan/equipment/gnss/Auxiliary.h>
 #include <uavcan/equipment/air_data/StaticPressure.h>
 #include <uavcan/equipment/air_data/StaticTemperature.h>
+#include <uavcan/equipment/air_data/RawAirData.h>
 #include <uavcan/equipment/indication/BeepCommand.h>
 #include <uavcan/equipment/indication/LightsCommand.h>
 #include <ardupilot/indication/SafetyState.h>
@@ -261,6 +262,9 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr
 #endif
 #ifdef HAL_PERIPH_ENABLE_BARO
         AP_Param::setup_object_defaults(&periph.baro, periph.baro.var_info);
+#endif
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+        AP_Param::setup_object_defaults(&periph.airspeed, periph.airspeed.var_info);
 #endif
     }
 
@@ -894,6 +898,7 @@ void AP_Periph_FW::can_update()
     can_mag_update();
     can_gps_update();
     can_baro_update();
+    can_airspeed_update();
 #ifdef HAL_PERIPH_ENABLE_BUZZER
     can_buzzer_update();
 #endif
@@ -1125,6 +1130,64 @@ void AP_Periph_FW::can_baro_update(void)
 #endif // HAL_PERIPH_ENABLE_BARO
 }
 
+
+/*
+  update CAN airspeed
+ */
+void AP_Periph_FW::can_airspeed_update(void)
+{
+#ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    if (!airspeed.healthy()) {
+        static uint32_t last_probe_ms;
+        uint32_t now = AP_HAL::millis();
+        if (now - last_probe_ms >= 1000) {
+            last_probe_ms = now;
+            airspeed.init();
+        }
+    }
+    airspeed.update(false);
+    if (last_airspeed_update_ms == airspeed.last_update_ms()) {
+        return;
+    }
+
+    last_airspeed_update_ms = airspeed.last_update_ms();
+    if (!airspeed.healthy()) {
+        // don't send any data
+        return;
+    }
+    const float press = airspeed.get_differential_pressure();
+    float temp;
+    if (!airspeed.get_temperature(temp)) {
+        temp = nanf("");
+    } else {
+        temp += C_TO_KELVIN;
+    }
+
+    uavcan_equipment_air_data_RawAirData pkt {};
+    pkt.differential_pressure = press;
+    pkt.static_air_temperature = temp;
+    fix_float16(pkt.differential_pressure);
+    fix_float16(pkt.static_air_temperature);
+
+    // unfilled elements are NaN
+    pkt.static_pressure = nanf("");
+    pkt.static_pressure_sensor_temperature = nanf("");
+    pkt.differential_pressure_sensor_temperature = nanf("");
+    pkt.pitot_temperature = nanf("");
+
+    uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE];
+    uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer);
+
+    canardBroadcast(&canard,
+                    UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,
+                    UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,
+                    &transfer_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &buffer[0],
+                    total_size);
+#endif // HAL_PERIPH_ENABLE_AIRSPEED
+}
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
 /*
   map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message
diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript
index ee66c80098..d23ff64fe8 100644
--- a/Tools/AP_Periph/wscript
+++ b/Tools/AP_Periph/wscript
@@ -32,6 +32,7 @@ def build(bld):
         'Filter',
         'AP_InternalError',
         'GCS_MAVLink',
+        'AP_Airspeed',
         ],
         exclude_src=[
             'libraries/AP_HAL_ChibiOS/Storage.cpp'