Pitot support

Makefile

@@ -477,6 +477,7 @@ HIGHEND_SRC = \
             io/dashboard.c \
             sensors/rangefinder.c \
             sensors/barometer.c \
+            sensors/pitotmeter.c \
             telemetry/telemetry.c \
             telemetry/frsky.c \
             telemetry/hott.c \

src/main/blackbox/blackbox.c

@@ -45,6 +45,7 @@
 #include "sensors/compass.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
 
@@ -207,6 +208,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
 #ifdef BARO
     {"BaroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
 #endif
+#ifdef PITOT
+    {"AirSpeed",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_PITOT},
+#endif
 #ifdef SONAR
     {"sonarRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
 #endif
@@ -332,6 +336,9 @@ typedef struct blackboxMainState_s {
 #ifdef BARO
     int32_t BaroAlt;
 #endif
+#ifdef PITOT
+    int32_t AirSpeed;
+#endif
 #ifdef MAG
     int16_t magADC[XYZ_AXIS_COUNT];
 #endif
@@ -470,6 +477,13 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
             return false;
 #endif
 
+        case FLIGHT_LOG_FIELD_CONDITION_PITOT:
+#ifdef PITOT
+            return sensors(SENSOR_PITOT);
+#else
+            return false;
+#endif
+
         case FLIGHT_LOG_FIELD_CONDITION_VBAT:
             return feature(FEATURE_VBAT);
 
@@ -605,6 +619,12 @@ static void writeIntraframe(void)
         }
 #endif
 
+#ifdef PITOT
+        if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_PITOT)) {
+            blackboxWriteSignedVB(blackboxCurrent->AirSpeed);
+        }
+#endif
+
 #ifdef SONAR
         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
             blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
@@ -767,6 +787,12 @@ static void writeInterframe(void)
     }
 #endif
 
+#ifdef PITOT
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_PITOT)) {
+        deltas[optionalFieldCount++] = blackboxCurrent->AirSpeed - blackboxLast->AirSpeed;
+    }
+#endif
+
 #ifdef SONAR
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
         deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
@@ -1108,6 +1134,10 @@ static void loadMainState(uint32_t currentTime)
     blackboxCurrent->BaroAlt = BaroAlt;
 #endif
 
+#ifdef PITOT
+    blackboxCurrent->AirSpeed = AirSpeed;
+#endif
+
 #ifdef SONAR
     // Store the raw sonar value without applying tilt correction
     blackboxCurrent->sonarRaw = rangefinderRead();

src/main/blackbox/blackbox_fielddefs.h

@@ -33,6 +33,7 @@ typedef enum FlightLogFieldCondition {
 
     FLIGHT_LOG_FIELD_CONDITION_MAG,
     FLIGHT_LOG_FIELD_CONDITION_BARO,
+    FLIGHT_LOG_FIELD_CONDITION_PITOT,
     FLIGHT_LOG_FIELD_CONDITION_VBAT,
     FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
     FLIGHT_LOG_FIELD_CONDITION_SONAR,

src/main/blackbox/blackbox_io.c

@@ -46,6 +46,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
 

src/main/config/config.c

@@ -42,6 +42,7 @@
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/pitotmeter.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"
@@ -242,6 +243,13 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig)
     barometerConfig->use_median_filtering = 1;
 }
 
+void resetPitotmeterConfig(pitotmeterConfig_t *pitotmeterConfig)
+{
+    pitotmeterConfig->pitot_sample_count = 21;
+    pitotmeterConfig->pitot_noise_lpf = 0.6f;
+    pitotmeterConfig->pitot_scale = 1.00f;
+}
+
 void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 {
     sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
@@ -576,6 +584,7 @@ static void resetConf(void)
     currentProfile->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions
 
     resetBarometerConfig(&masterConfig.barometerConfig);
+    resetPitotmeterConfig(&masterConfig.pitotmeterConfig);
 
     // Radio
 #ifdef RX_CHANNELS_TAER
@@ -802,6 +811,9 @@ void activateConfig(void)
 #ifdef BARO
     useBarometerConfig(&masterConfig.barometerConfig);
 #endif
+#ifdef PITOT
+    usePitotmeterConfig(&masterConfig.pitotmeterConfig);
+#endif
 }
 
 void validateAndFixConfig(void)

src/main/config/config_eeprom.c

@@ -36,6 +36,7 @@
 #include "sensors/barometer.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+#include "sensors/pitotmeter.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"

src/main/config/config_master.h

@@ -108,6 +108,8 @@ typedef struct master_s {
 
     barometerConfig_t barometerConfig;
 
+    pitotmeterConfig_t pitotmeterConfig;
+
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
     uint8_t baro_hardware;                  // Barometer hardware to use
 

src/main/drivers/pitotmeter.h

@@ -0,0 +1,32 @@
+/*
+ * 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
+
+typedef void (*pitotOpFuncPtr)(void);                       // pitot start operation
+typedef void (*pitotCalculateFuncPtr)(float *pressure, float *temperature); // airspeed calculation (filled params are pressure and temperature)
+
+typedef struct pitot_t {
+    uint16_t delay;
+    pitotOpFuncPtr start;
+    pitotOpFuncPtr get;
+    pitotCalculateFuncPtr calculate;
+} pitot_t;
+
+#ifndef PITOT_I2C_INSTANCE
+#define PITOT_I2C_INSTANCE I2C_DEVICE
+#endif

src/main/drivers/pitotmeter_ms4525.c

@@ -0,0 +1,134 @@
+/*
+ * 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/build_config.h"
+
+#include "pitotmeter.h"
+
+#include "system.h"
+#include "bus_i2c.h"
+
+// MS4525, Standard address 0x28
+#define MS4525_ADDR                 0x28
+//#define MS4525_ADDR                 0x36
+//#define MS4525_ADDR                 0x46
+
+static void ms4525_start(void);
+static void ms4525_read(void);
+static void ms4525_calculate(float *pressure, float *temperature);
+
+static uint16_t ms4525_ut;  // static result of temperature measurement
+static uint16_t ms4525_up;  // static result of pressure measurement
+static uint16_t zeropointvalue;  // measured zeropoint
+static bool zeropoint = false;
+static uint16_t calibrationCount = 0;
+static uint32_t filter_reg = 0; // Barry Dorr filter register
+
+bool ms4525Detect(pitot_t *pitot)
+{
+    bool ack = false;
+    uint8_t sig;
+
+    // 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 = i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 1, &sig );
+    ack = i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 1, &sig );
+    if (!ack)
+        return false;
+
+    pitot->delay = 10000;
+    pitot->start = ms4525_start;
+    pitot->get = ms4525_read;
+    pitot->calculate = ms4525_calculate;
+    ms4525_read();
+    return true;
+}
+
+static void ms4525_start(void)
+{
+    uint8_t sig;
+    i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 1, &sig );
+}
+
+static void ms4525_read(void)
+{
+    uint8_t rxbuf[4];
+    i2cRead( PITOT_I2C_INSTANCE, MS4525_ADDR, 0xFF, 4, rxbuf ); // read ADC
+    ms4525_up = (rxbuf[0] << 8) | (rxbuf[1] << 0);
+    ms4525_ut = ((rxbuf[2] << 8) | (rxbuf[3] << 0))>>5;
+}
+
+
+void voltage_correction(float *pressure, float *temperature)
+{
+    UNUSED(pressure);
+    UNUSED(temperature);
+}
+
+static void ms4525_calculate(float *pressure, float *temperature)
+{
+    uint8_t status = (ms4525_up & 0xC000) >> 14;
+    switch (status) {
+        case 0:
+            break;
+        case 1:
+            /* fallthrough */
+        case 2:
+            /* fallthrough */
+        case 3:
+            return;
+    }
+    int16_t dp_raw = 0, dT_raw = 0;
+
+    /* mask the used bits */
+    dp_raw = 0x3FFF & ms4525_up;
+    dT_raw = ms4525_ut;
+
+    if (!zeropoint) {
+       if (calibrationCount <= 0) {
+           calibrationCount++;
+           filter_reg = (dp_raw << 5);
+           return;
+       } else if (calibrationCount <= 100) {
+           calibrationCount++;
+           filter_reg = filter_reg - (filter_reg >> 5) + dp_raw;
+           if (calibrationCount > 100) {
+               zeropointvalue = (uint16_t)(filter_reg >> 5);
+               zeropoint=true;
+               calibrationCount = 0;
+           }
+           return;
+       }
+    }
+
+    float dP = (10 * (int32_t)(dp_raw - zeropointvalue)) * 0.1052120688f;
+    float T  = (float)(200 * (int32_t)dT_raw - 102350) / 2047 + 273.15f;
+
+    if (pressure)
+        *pressure = dP;	// Pa
+    if (temperature)
+        *temperature = T; // K
+}
+

src/main/drivers/pitotmeter_ms4525.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 ms4525Detect(pitot_t *pitot);

src/main/fc/fc_msp.c

@@ -70,6 +70,7 @@
 #include "sensors/rangefinder.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 

src/main/fc/fc_tasks.c

@@ -63,6 +63,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 
 #include "telemetry/telemetry.h"
@@ -82,7 +83,7 @@
 
 // VBAT monitoring interval (in microseconds) - 1s
 #define VBATINTERVAL (6 * 3500)
-// IBat monitoring interval (in microseconds) - 6 default looptimes
+/* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)
 
 void taskHandleSerial(uint32_t currentTime)
@@ -167,6 +168,17 @@ void taskUpdateBaro(uint32_t currentTime)
 }
 #endif
 
+#ifdef PITOT
+void taskUpdatePitot(uint32_t currentTime)
+{
+    UNUSED(currentTime);
+
+    if (sensors(SENSOR_PITOT)) {
+        pitotUpdate();
+    }
+}
+#endif
+
 #ifdef SONAR
 void taskUpdateSonar(uint32_t currentTime)
 {
@@ -342,6 +354,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef PITOT
+    [TASK_PITOT] = {
+        .taskName = "PITOT",
+        .taskFunc = taskUpdatePitot,
+        .desiredPeriod = 1000000 / 10,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 #ifdef SONAR
     [TASK_SONAR] = {
         .taskName = "SONAR",
@@ -442,6 +463,9 @@ void fcTasksInit(void)
 #ifdef BARO
     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 #endif
+#ifdef PITOT
+    setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
+#endif
 #ifdef SONAR
     setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
 #endif

src/main/fc/mw.c

@@ -40,6 +40,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
 

src/main/flight/navigation_rewrite_pos_estimator.c

@@ -35,6 +35,7 @@
 #include "sensors/sensors.h"
 #include "sensors/rangefinder.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
@@ -62,6 +63,7 @@
 
 #define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
 #define INAV_BARO_UPDATE_RATE               20
+#define INAV_PITOT_UPDATE_RATE              10
 #define INAV_SONAR_UPDATE_RATE              15      // Sonar is limited to 1/60ms update rate, go lower that that
 
 #define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
@@ -96,6 +98,11 @@ typedef struct {
     float       epv;
 } navPositionEstimatorBARO_t;
 
+typedef struct {
+    uint32_t    lastUpdateTime; // Last update time (us)
+    float       airspeed;            // airspeed (cm/s)
+} navPositionEstimatorPITOT_t;
+
 typedef struct {
     uint32_t    lastUpdateTime; // Last update time (us)
     float       alt;            // Raw altitude measurement (cm)
@@ -134,6 +141,7 @@ typedef struct {
     navPositionEstimatorGPS_t   gps;
     navPositionEstimatorBARO_t  baro;
     navPositionEstimatorSONAR_t sonar;
+    navPositionEstimatorPITOT_t pitot;
 
     // IMU data
     navPosisitonEstimatorIMU_t  imu;
@@ -368,6 +376,28 @@ static void updateBaroTopic(uint32_t currentTime)
 }
 #endif
 
+#if defined(PITOT)
+/**
+ * Read Pitot and update airspeed topic
+ *  Function is called at main loop rate, updates happen at reduced rate
+ */
+static void updatePitotTopic(uint32_t currentTime)
+{
+    static navigationTimer_t pitotUpdateTimer;
+
+    if (updateTimer(&pitotUpdateTimer, HZ2US(INAV_PITOT_UPDATE_RATE), currentTime)) {
+        float newTAS = pitotCalculateAirSpeed();
+        if (sensors(SENSOR_PITOT) && isPitotCalibrationComplete()) {
+            posEstimator.pitot.airspeed = newTAS;
+        }
+        else {
+            posEstimator.pitot.airspeed = 0;
+        }
+    }
+}
+#endif
+
+
 #if defined(SONAR)
 /**
  * Read sonar and update alt/vel topic
@@ -742,6 +772,10 @@ void updatePositionEstimator(void)
     updateBaroTopic(currentTime);
 #endif
 
+#if defined(PITOT)
+    updatePitotTopic(currentTime);
+#endif
+
 #if defined(SONAR)
     updateSonarTopic(currentTime);
 #endif

src/main/io/ledstrip.c

@@ -48,6 +48,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
+#include "sensors/pitotmeter.h"
 
 #include "io/ledstrip.h"
 #include "io/beeper.h"
@@ -73,7 +74,6 @@
 #include "config/config_profile.h"
 #include "config/feature.h"
 
-
 /*
 PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0);
 PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0);
@@ -81,6 +81,7 @@ PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG
 PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0);
 */
 
+
 static bool ledStripInitialised = false;
 static bool ledStripEnabled = true;
 

src/main/io/serial_cli.c

@@ -76,6 +76,7 @@
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 
 #include "flight/pid.h"
 #include "flight/imu.h"
@@ -841,6 +842,12 @@ const clivalue_t valueTable[] = {
     { "baro_hardware",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.baro_hardware, .config.minmax = { 0,  BARO_MAX }, 0 },
 #endif
 
+#ifdef PITOT
+    { "pitot_tab_size",             VAR_UINT8  | MASTER_VALUE, &masterConfig.pitotmeterConfig.pitot_sample_count, .config.minmax = { 0, PITOT_SAMPLE_COUNT_MAX }, 0 },
+    { "pitot_noise_lpf",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.pitotmeterConfig.pitot_noise_lpf, .config.minmax = { 0, 1 }, 0 },
+    { "pitot_scale",                VAR_FLOAT  | MASTER_VALUE, &masterConfig.pitotmeterConfig.pitot_scale, .config.minmax = { 0, 100 }, 0 },
+#endif
+
 #ifdef MAG
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, .config.minmax = { 0,  MAG_MAX }, 0 },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000,  18000 }, 0 },

src/main/scheduler/scheduler.h

@@ -63,6 +63,9 @@ typedef enum {
 #ifdef BARO
     TASK_BARO,
 #endif
+#ifdef PITOT
+    TASK_PITOT,
+#endif
 #ifdef SONAR
     TASK_SONAR,
 #endif

src/main/sensors/initialisation.c

@@ -57,6 +57,9 @@
 #include "drivers/barometer_fake.h"
 #include "drivers/barometer_ms5611.h"
 
+#include "drivers/pitotmeter.h"
+#include "drivers/pitotmeter_ms4525.h"
+
 #include "drivers/compass.h"
 #include "drivers/compass_ak8963.h"
 #include "drivers/compass_ak8975.h"
@@ -77,6 +80,7 @@
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
 #include "sensors/rangefinder.h"
@@ -89,6 +93,7 @@
 extern baro_t baro;
 extern mag_t mag;
 extern sensor_align_e gyroAlign;
+extern pitot_t pitot;
 
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
 
@@ -455,6 +460,21 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
 }
 #endif // BARO
 
+#ifdef PITOT
+static bool detectPitot()
+{
+    // Detect what pressure sensors are available.
+#ifdef USE_PITOT_MS4525
+    if (ms4525Detect(&pitot)) {
+        sensorsSet(SENSOR_PITOT);
+        return true;
+    }
+#endif
+    sensorsClear(SENSOR_PITOT);
+    return false;
+}
+#endif
+
 #ifdef MAG
 static bool detectMag(magSensor_e magHardwareToUse)
 {
@@ -673,6 +693,10 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     UNUSED(baroHardwareToUse);
 #endif
 
+#ifdef PITOT
+    detectPitot();
+#endif
+
     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
     magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
 #ifdef MAG

src/main/sensors/pitotmeter.c

@@ -0,0 +1,150 @@
+/*
+ * 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 <math.h>
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "drivers/pitotmeter.h"
+#include "config/config.h"
+
+#include "sensors/pitotmeter.h"
+
+extern int16_t debug[4];
+
+pitot_t pitot;                  // pitotmeter access functions
+int32_t AirSpeed = 0;
+
+#ifdef PITOT
+
+static float pitotPressureSum = 0;
+static uint16_t calibratingP = 0;      // baro calibration = get new ground pressure value
+static float pitotPressure = 0;
+static float pitotTemperature = 0;
+static float CalibratedAirspeed = 0;
+
+pitotmeterConfig_t *pitotmeterConfig;
+
+void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse)
+{
+    pitotmeterConfig = pitotmeterConfigToUse;
+}
+
+bool isPitotCalibrationComplete(void)
+{
+    return calibratingP == 0;
+}
+
+void pitotSetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    calibratingP = calibrationCyclesRequired;
+}
+
+static bool pitotReady = false;
+
+#define PRESSURE_SAMPLE_COUNT (pitotmeterConfig->pitot_sample_count - 1)
+
+static float recalculatePitotmeterTotal(uint8_t pitotSampleCount, float pressureTotal, float newPressureReading)
+{
+    static float pitotmeterSamples[PITOT_SAMPLE_COUNT_MAX];
+    static int currentSampleIndex = 0;
+    int nextSampleIndex;
+
+    // store current pressure in pitotmeterSamples
+    nextSampleIndex = (currentSampleIndex + 1);
+    if (nextSampleIndex == pitotSampleCount) {
+        nextSampleIndex = 0;
+        pitotReady = true;
+    }
+    pitotmeterSamples[currentSampleIndex] = newPressureReading;
+
+    // recalculate pressure total
+    // Note, the pressure total is made up of pitotSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT
+    pressureTotal += pitotmeterSamples[currentSampleIndex];
+    pressureTotal -= pitotmeterSamples[nextSampleIndex];
+
+    currentSampleIndex = nextSampleIndex;
+
+    return pressureTotal;
+}
+
+typedef enum {
+    PITOTMETER_NEEDS_SAMPLES = 0,
+    PITOTMETER_NEEDS_CALCULATION,
+    PITOTMETER_NEEDS_PROCESSING
+} pitotmeterState_e;
+
+
+bool isPitotReady(void) {
+    return pitotReady;
+}
+
+uint32_t pitotUpdate(void)
+{
+    static pitotmeterState_e state = PITOTMETER_NEEDS_SAMPLES;
+
+    switch (state) {
+        default:
+        case PITOTMETER_NEEDS_SAMPLES:
+            pitot.start();
+            state = PITOTMETER_NEEDS_CALCULATION;
+            return pitot.delay;
+        break;
+
+        case PITOTMETER_NEEDS_CALCULATION:
+            pitot.get();
+            pitot.calculate(&pitotPressure, &pitotTemperature);
+            state = PITOTMETER_NEEDS_PROCESSING;
+            return pitot.delay;
+        break;
+
+        case PITOTMETER_NEEDS_PROCESSING:
+            state = PITOTMETER_NEEDS_SAMPLES;
+            pitotPressureSum = recalculatePitotmeterTotal(pitotmeterConfig->pitot_sample_count, pitotPressureSum, pitotPressure);
+            return pitot.delay;
+        break;
+    }
+}
+
+#define P0          101325.0f           // standard pressure
+#define CCEXPONENT  0.2857142857f       // exponent of compressibility correction 2/7
+#define CASFACTOR   760.8802669f        // sqrt(5) * speed of sound at standard
+#define TASFACTOR   0.05891022589f      // 1/sqrt(T0)
+
+int32_t pitotCalculateAirSpeed(void)
+{
+    float CalibratedAirspeed_tmp;
+    CalibratedAirspeed_tmp = pitotmeterConfig->pitot_scale * CASFACTOR * sqrtf(powf(fabsf(pitotPressureSum / PRESSURE_SAMPLE_COUNT) / P0 + 1.0f, CCEXPONENT) - 1.0f);
+    //const float CalibratedAirspeed = 1 * CASFACTOR * sqrtf(powf(fabsf(pitotPressure) / P0 + 1.0f, CCEXPONENT) - 1.0f);
+
+    CalibratedAirspeed = CalibratedAirspeed * pitotmeterConfig->pitot_noise_lpf + CalibratedAirspeed_tmp * (1.0f - pitotmeterConfig->pitot_noise_lpf); // additional LPF to reduce baro noise
+    float TrueAirspeed = CalibratedAirspeed * TASFACTOR * sqrtf(pitotTemperature);
+
+    AirSpeed = TrueAirspeed*100;
+    //debug[0] = (int16_t)(CalibratedAirspeed*100);
+    //debug[1] = (int16_t)(TrueAirspeed*100);
+    //debug[2] = (int16_t)((pitotTemperature-273.15f)*100);
+    debug[3] = AirSpeed;
+
+    return AirSpeed;
+}
+
+#endif /* PITOT */

src/main/sensors/pitotmeter.h

@@ -0,0 +1,38 @@
+/*
+ * 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
+
+#define PITOT_SAMPLE_COUNT_MAX   48
+
+typedef struct pitotmeterConfig_s {
+    uint8_t pitot_sample_count;              // size of pitot filter array
+    float pitot_noise_lpf;                   // additional LPF to reduce pitot noise
+    float pitot_scale;                       // scale value
+} pitotmeterConfig_t;
+
+extern int32_t AirSpeed;
+
+#ifdef PITOT
+void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse);
+bool isPitotCalibrationComplete(void);
+void pitotSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+uint32_t pitotUpdate(void);
+bool isPitotReady(void);
+int32_t pitotCalculateAirSpeed(void);
+void performPitotCalibrationCycle(void);
+#endif

src/main/sensors/sensors.h

@@ -49,6 +49,7 @@ typedef enum {
     SENSOR_SONAR = 1 << 4,
     SENSOR_GPS = 1 << 5,
     SENSOR_GPSMAG = 1 << 6,
+    SENSOR_PITOT = 1 << 7,
 } sensors_e;
 
 typedef enum {

src/main/target/ANYFC/target.h

@@ -97,6 +97,7 @@
 #define USE_I2C
 #define I2C_DEVICE              (I2CDEV_1)
 #define I2C_DEVICE_EXT          (I2CDEV_2)
+//#define USE_I2C_PULLUP
 
 //#define HIL
 

src/main/target/ANYFC/target.mk

@@ -6,5 +6,6 @@ TARGET_SRC = \
             drivers/barometer_ms5611.c \
             drivers/compass_hmc5883l.c \
             drivers/light_ws2811strip.c \
-            drivers/light_ws2811strip_stm32f4xx.c
+            drivers/light_ws2811strip_stm32f4xx.c \
+            drivers/pitotmeter_ms4525.c
 

src/main/target/STM32F3DISCOVERY/target.h

@@ -85,6 +85,10 @@
 #define USE_MAG_HMC5883
 #define USE_MAG_MAG3110
 
+#define PITOT
+#define USE_PITOT_MS4525
+#define PITOT_I2C_INSTANCE I2C_DEVICE
+
 #define USE_VCP
 #define USE_UART1
 #define USE_UART2

src/main/target/STM32F3DISCOVERY/target.mk

@@ -26,5 +26,6 @@ TARGET_SRC = \
             drivers/compass_fake.c \
             drivers/compass_mag3110.c \
             drivers/flash_m25p16.c \
-            drivers/light_ws2811strip.c
+            drivers/light_ws2811strip.c \
+            drivers/pitotmeter_ms4525.c
 

src/main/telemetry/mavlink.c

@@ -52,6 +52,7 @@
 #include "sensors/barometer.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+#include "sensors/pitotmeter.h"
 
 #include "rx/rx.h"