Pitot DLVR-L10D sensor

src/main/CMakeLists.txt

@@ -208,6 +208,8 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_adc.h
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.h	
     drivers/pitotmeter/pitotmeter_msp.c
     drivers/pitotmeter/pitotmeter_msp.h
     drivers/pitotmeter/pitotmeter_virtual.c

src/main/common/calibration.c

@@ -30,6 +30,7 @@
 #include "drivers/time.h"
 #include "common/calibration.h"
 
+
 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
 {
     // Reset parameters and state
@@ -75,9 +76,11 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
     // Check if calibration is complete
     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
         const float stddev = devStandardDeviation(&s->val.stdDev);
+
         if (stddev > s->params.stdDevThreshold) {
             if (!s->params.allowFailure) {
-                // If deviation is too big - restart calibration
+                // If deviation is too big && no failure allowed - restart calibration
+                // TODO :: some safeguard should exist which will not allow to keep on calibrating for ever
                 s->params.startTimeMs = millis();
                 s->params.sampleCount = 0;
                 s->val.accumulatedValue = 0;

src/main/drivers/bus.h

@@ -138,6 +138,7 @@ typedef enum {
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter
+    DEVHW_DLVR,         // Pitot meter
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_W25N01G,      // SPI 128MB flash
     DEVHW_UG2864,       // I2C OLED display

src/main/drivers/pitotmeter/pitotmeter.h

@@ -26,6 +26,7 @@ typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure
 typedef struct pitotDev_s {
     busDevice_t * busDev;
     uint16_t delay;
+    float calibThreshold;
     pitotOpFuncPtr start;
     pitotOpFuncPtr get;
     pitotCalculateFuncPtr calculate;

src/main/drivers/pitotmeter/pitotmeter_adc.c

@@ -67,6 +67,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera
 bool adcPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // TODO :: should be tested !!!
     pitot->start = adcPitotStart;
     pitot->get = adcPitotRead;
     pitot->calculate = adcPitotCalculate;

src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c

@@ -0,0 +1,165 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+#include <build/debug.h>
+
+#include "common/log.h"
+#include "common/utils.h"
+#include "common/maths.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+
+#define DLVR_L10D_ADDR                 0x28     // this var is not used !!!
+
+// //---------------------------------------------------
+// // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
+// #define ISA_GAS_CONSTANT 287.26f
+// #define ISA_LAPSE_RATE 0.0065f
+
+// // Standard Sea Level values
+// // Ref: https://en.wikipedia.org/wiki/Standard_sea_level
+// #define SSL_AIR_DENSITY         1.225f // kg/m^3
+// #define SSL_AIR_PRESSURE 101325.01576f // Pascal
+// #define SSL_AIR_TEMPERATURE    288.15f // K
+// //---------------------------------------------------
+
+#define INCH_OF_H2O_TO_PASCAL   248.84f
+
+#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
+
+#define RANGE_INCH_H2O      10
+#define DLVR_OFFSET         8192.0f
+#define DLVR_SCALE          16384.0f
+// NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration
+#define DLVR_OFFSET_CORR    0.0f   //-9.0f
+
+
+typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
+    bool     dataValid;
+    uint32_t dlvr_ut;
+    uint32_t dlvr_up;
+} dlvrCtx_t;
+
+STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
+
+static bool dlvr_start(pitotDev_t * pitot)
+{
+    UNUSED(pitot);
+    return true;
+}
+
+static bool dlvr_read(pitotDev_t * pitot)
+{
+    uint8_t rxbuf1[4];
+
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+
+    if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
+        return false;
+    }
+
+    // status = 00 -> ok, new data
+	// status = 01 -> reserved
+    // status = 10 -> ok, data stale
+    // status = 11 -> error
+    const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
+
+    if (status) {
+        // anything other then 00 in the status bits is an error
+        LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) );
+        return false;
+    }
+
+    int16_t dP_raw1, dT_raw1;
+
+    dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
+    dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
+
+    // Data valid, update ut/up values
+    ctx->dataValid = true;
+    ctx->dlvr_up = dP_raw1;
+    ctx->dlvr_ut = dT_raw1;
+
+    return true;
+}
+
+static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
+{
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+
+
+    // pressure in inchH2O
+    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
+
+    // temperature in deg C
+    float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
+
+    // result must fit inside the max pressure range
+    if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
+        LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) );
+        return;
+    }
+
+    if (pressure) {
+        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);   // Pa
+    }
+
+    if (temperature) {
+        *temperature = C_TO_KELVIN( T_C); // K
+    }
+}
+
+bool dlvrDetect(pitotDev_t * pitot)
+{
+    uint8_t rxbuf[4];
+    bool ack = false;
+
+    pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED);
+    if (pitot->busDev == NULL) {
+        return false;
+    }
+
+    // Read twice to fix:
+    // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
+    // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
+    // An additional start condition must be sent, which results in restoration of proper communication.
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    if (!ack) {
+        return false;
+    }
+
+    // Initialize busDev data
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+    ctx->dlvr_ut = 0;
+    ctx->dlvr_up = 0;
+
+    // Initialize pitotDev object
+    pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // low noise sensor
+    pitot->start = dlvr_start;
+    pitot->get = dlvr_read;
+    pitot->calculate = dlvr_calculate;
+    return true;
+}

src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h

@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool dlvrDetect(pitotDev_t *pitot);

src/main/drivers/pitotmeter/pitotmeter_fake.c

@@ -75,6 +75,7 @@ bool fakePitotDetect(pitotDev_t *pitot)
     fakeTemperature = 273;      // 0C
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = fakePitotStart;
     pitot->get = fakePitotRead;
     pitot->calculate = fakePitotCalculate;

src/main/drivers/pitotmeter/pitotmeter_ms4525.c

@@ -74,6 +74,13 @@ static bool ms4525_read(pitotDev_t * pitot)
     dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
     dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
 
+    // reject any values that are the absolute minimum or maximums these
+    // can happen due to gnd lifts or communication errors on the bus
+    if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 ||
+        dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
+        return false;
+    }
+
     // reject any double reads where the value has shifted in the upper more than 0xFF
     if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
         return false;
@@ -83,6 +90,7 @@ static bool ms4525_read(pitotDev_t * pitot)
     ctx->dataValid = true;
     ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
     ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
+
     return true;
 }
 
@@ -136,6 +144,7 @@ bool ms4525Detect(pitotDev_t * pitot)
 
     // Initialize pitotDev object
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00005f;   // noisy sensor
     pitot->start = ms4525_start;
     pitot->get = ms4525_read;
     pitot->calculate = ms4525_calculate;

src/main/drivers/pitotmeter/pitotmeter_msp.c

@@ -87,6 +87,7 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
     mspPitotTemperature = 27315;    // 0 deg/c
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = mspPitotStart;
     pitot->get = mspPitotRead;
     pitot->calculate = mspPitotCalculate;
@@ -95,3 +96,4 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
 }
 
 #endif
+

src/main/drivers/pitotmeter/pitotmeter_virtual.c

@@ -88,6 +88,7 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem
 bool virtualPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = virtualPitotStart;
     pitot->get = virtualPitotRead;
     pitot->calculate = virtualPitotCalculate;

src/main/fc/fc_tasks.c

@@ -494,7 +494,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_BARO] = {
         .taskName = "BARO",
         .taskFunc = taskUpdateBaro,
-        .desiredPeriod = TASK_PERIOD_HZ(20),
+        .desiredPeriod = TASK_PERIOD_MS(50),        // SPL06 uses oversampilng - result 16ms ?
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
@@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .desiredPeriod = TASK_PERIOD_MS(20),
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif

src/main/fc/settings.yaml

@@ -19,7 +19,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]