AP_Baro: added qflight driver

libraries/AP_Baro/AP_Baro.cpp

@@ -28,6 +28,7 @@
 #include "AP_Baro_HIL.h"
 #include "AP_Baro_MS5611.h"
 #include "AP_Baro_PX4.h"
+#include "AP_Baro_qflight.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -327,6 +328,11 @@ void AP_Baro::init(void)
         drivers[0] = new AP_Baro_MS5637(*this, bus, true);
         _num_drivers = 1;
     }
+#elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT
+    {
+        drivers[0] = new AP_Baro_QFLIGHT(*this);
+        _num_drivers = 1;
+    }
 #endif
     if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) {
         AP_HAL::panic("Baro: unable to initialise driver");

libraries/AP_Baro/AP_Baro_qflight.cpp

@@ -0,0 +1,68 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+#include <unistd.h>
+#include <stdio.h>
+#include "AP_Baro_qflight.h"
+#include <AP_HAL_Linux/qflight/qflight_util.h>
+#include <AP_HAL_Linux/qflight/qflight_dsp.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor - opens the QFLIGHT drivers
+ */
+AP_Baro_QFLIGHT::AP_Baro_QFLIGHT(AP_Baro &baro) :
+    AP_Baro_Backend(baro)
+{
+    instance = _frontend.register_sensor();
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_QFLIGHT::timer_update, void));
+    hal.scheduler->delay(100);
+}
+
+// Read the sensor
+void AP_Baro_QFLIGHT::timer_update(void)
+{
+    uint32_t now = AP_HAL::millis();
+    if (now - last_check_ms < 25) {
+        return;
+    }
+    last_check_ms = now;
+    
+    if (barobuf == nullptr) {
+        barobuf = QFLIGHT_RPC_ALLOCATE(DSPBuffer::BARO);
+        if (barobuf == nullptr) {
+            AP_HAL::panic("Unable to allocate baro buffer");
+        }
+    }
+    int ret = qflight_get_baro_data((uint8_t *)barobuf, sizeof(*barobuf));
+    if (ret != 0) {
+        return;
+    }
+
+    for (uint16_t i=0; i<barobuf->num_samples; i++) {
+        DSPBuffer::BARO::BUF &b = barobuf->buf[i];
+        pressure_sum += b.pressure_pa;
+        temperature_sum += b.temperature_C;
+        sum_count++;
+    }
+}
+
+// Read the sensor
+void AP_Baro_QFLIGHT::update(void)
+{
+    if (sum_count > 0) {
+        hal.scheduler->suspend_timer_procs();
+        _copy_to_frontend(instance,
+                          pressure_sum/sum_count,
+                          temperature_sum/sum_count);
+        sum_count = 0;
+        pressure_sum = 0;
+        temperature_sum = 0;
+        hal.scheduler->resume_timer_procs();
+    }
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_Baro/AP_Baro_qflight.h

@@ -0,0 +1,26 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+#include "AP_Baro_Backend.h"
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+#include <AP_HAL_Linux/qflight/qflight_buffer.h>
+
+class AP_Baro_QFLIGHT : public AP_Baro_Backend
+{
+public:
+    AP_Baro_QFLIGHT(AP_Baro &);
+    void update();
+
+private:
+    DSPBuffer::BARO *barobuf;
+    uint8_t instance;
+    float pressure_sum;
+    float temperature_sum;
+    uint32_t sum_count;
+    uint32_t last_check_ms;
+    
+    void timer_update();
+};
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE