LP-623 Added suport SDP3x I2C Differential Pressure/Airspeed sensor on Flexi Port - tested

flight/make/apps-defs.mk

@@ -74,6 +74,7 @@ SRC += $(PIOSCOMMON)/pios_adxl345.c
 SRC += $(PIOSCOMMON)/pios_bma180.c
 SRC += $(PIOSCOMMON)/pios_bmp085.c
 SRC += $(PIOSCOMMON)/pios_etasv3.c
+SRC += $(PIOSCOMMON)/pios_sdp3x.c
 SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
 SRC += $(PIOSCOMMON)/pios_hcsr04.c
 SRC += $(PIOSCOMMON)/pios_hmc5843.c

flight/modules/Airspeed/airspeed.c

@@ -43,6 +43,7 @@
 #include "airspeedsensor.h" // object that will be updated by the module
 #include "baro_airspeed_ms4525do.h"
 #include "baro_airspeed_etasv3.h"
+#include "baro_airspeed_sdp3x.h"
 #include "baro_airspeed_mpxv.h"
 #include "imu_airspeed.h"
 #include "airspeedalarm.h"
@@ -186,6 +187,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
             baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
             break;
 #endif
+#if defined(PIOS_INCLUDE_SDP3X)
+        case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_SDP3X:
+            // SDP3X
+            baro_airspeedGetSDP3X(&airspeedData, &airspeedSettings);
+            break;
+#endif
 #if defined(PIOS_INCLUDE_MS4525DO)
         case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
             // PixHawk Airpeed based on MS4525DO

flight/modules/Airspeed/baro_airspeed_sdp3x.c

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Communicate with airspeed sensors and return values
+ * @{
+ *
+ * @file       baro_airspeed.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Airspeed module, handles temperature and pressure readings from BMP085
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: BaroAirspeed
+ *
+ * This module will periodically update the value of the BaroAirspeed object.
+ *
+ */
+
+#include "openpilot.h"
+#include "hwsettings.h"
+#include "airspeedsettings.h"
+#include "airspeedsensor.h" // object that will be updated by the module
+#include "airspeedalarm.h"
+
+#if defined(PIOS_INCLUDE_SDP3X)
+
+#define CALIBRATION_IDLE_MS  2000   // Time to wait before calibrating, in [ms]
+#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms]
+
+#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)
+
+// Private types
+
+// Private variables
+
+// Private functions
+
+static volatile struct PIOS_SDP3X_STATE state = { .status = SDP3X_STATUS_NOTFOUND, .pressure = 0, .temperature = 0 };
+
+void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
+{
+    // Check to see if airspeed sensor is returning airspeedSensor
+    PIOS_SDP3X_ReadAirspeed(&state);
+    airspeedSensor->SensorValue = state.pressure;
+    airspeedSensor->SensorValueTemperature = state.temperature;
+
+    if (state.status != SDP3X_STATUS_READY) {
+        airspeedSensor->SensorConnected    = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
+        airspeedSensor->CalibratedAirspeed = 0;
+        AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
+        return;
+    }
+
+    // No calibration, sensor comes factory calibrated!
+
+    // Compute airspeed
+    airspeedSensor->Temperature          = ((float)state.temperature / 200.0f) + 273.15f; // convert to kelvin
+    airspeedSensor->DifferentialPressure = (1.0f / (((float)state.scale) + 1e-9f)) * ((float)(state.pressure) - (float)((int16_t)(airspeedSettings->ZeroPoint)));
+    airspeedSensor->CalibratedAirspeed   = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(airspeedSensor->DifferentialPressure) / P0 + 1.0f, CCEXPONENT) - 1.0f);
+    airspeedSensor->TrueAirspeed         = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(airspeedSensor->Temperature);
+
+    airspeedSensor->SensorConnected      = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
+    AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
+}
+
+
+#endif /* if defined(PIOS_INCLUDE_SDP3X) */
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Airspeed/inc/baro_airspeed_sdp3x.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup AirspeedModule Airspeed Module
+ * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
+ * @{
+ *
+ * @file       baro_airspeed_etasv3.h
+ * @author     The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
+ * @brief      Airspeed module, reads temperature and pressure from SDP3X
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef BARO_AIRSPEED_SDP3X_H
+#define BARO_AIRSPEED_SDP3X_H
+#if defined(PIOS_INCLUDE_SDP3X)
+
+void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
+
+#endif
+#endif // BARO_AIRSPEED_SDP3X_H
+
+/**
+ * @}
+ * @}
+ */

flight/pios/common/pios_sdp3x.c

@@ -0,0 +1,126 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors
+ * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3
+ * @{
+ *
+ * @file       pios_stp3x.c
+ * @author     The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
+ * @brief      SDP3x Airspeed Sensor Driver
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ ******************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "pios.h"
+
+#ifdef PIOS_INCLUDE_SDP3X
+
+static bool PIOS_SDP3X_WriteCommand(uint16_t command)
+{
+    uint8_t commandbuffer[2];
+
+    commandbuffer[0] = (uint8_t)((uint16_t)(command >> 8));
+    commandbuffer[1] = (uint8_t)((uint16_t)(command & 0xff));
+    const struct pios_i2c_txn txn_list[] = {
+        {
+            .info = __func__,
+            .addr = SDP3X_I2C_ADDR1,
+            .rw   = PIOS_I2C_TXN_WRITE,
+            .len  = 2,
+            .buf  = commandbuffer,
+        }
+    };
+
+    return PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list));
+}
+
+static bool PIOS_SDP3X_Read(uint8_t *buffer, uint8_t len)
+{
+    const struct pios_i2c_txn txn_list[] = {
+        {
+            .info = __func__,
+            .addr = SDP3X_I2C_ADDR1,
+            .rw   = PIOS_I2C_TXN_READ,
+            .len  = len,
+            .buf  = buffer,
+        }
+    };
+
+    for (uint8_t retry = PIOS_SDP3X_RETRY_LIMIT; retry > 0; --retry) {
+        if (PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0) {
+            return 0;
+        }
+    }
+    return -1;
+}
+
+uint8_t PIOS_SDP3X_crc(const uint8_t buffer[], unsigned len, uint8_t crc)
+{
+    uint8_t c = SDP3X_CRC_INIT;
+
+    for (uint8_t i = 0; i < len; i++) {
+        c ^= buffer[i];
+        for (uint8_t b = 8; b > 0; --b) {
+            if (c & 0x80) {
+                c = (c << 1) ^ SDP3X_CRC_POLY;
+            } else {
+                c = c << 1;
+            }
+        }
+    }
+    return c == crc;
+}
+
+void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state)
+{
+    PIOS_Assert(state);
+
+    if (state->status == SDP3X_STATUS_NOTFOUND) {
+        if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MODE_STOP) == 0) {
+            PIOS_DELAY_WaituS(500);
+            if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MEAS_AVG_MODE) == 0) {
+                state->status = SDP3X_STATUS_CONFIGURED;
+            }
+        }
+        return;
+    }
+    if (state->status == SDP3X_STATUS_CONFIGURED || state->status == SDP3X_STATUS_READY) {
+        state->status = SDP3X_STATUS_READY;
+        uint8_t airspeed_raw[9];
+        if (PIOS_SDP3X_Read(airspeed_raw, sizeof(airspeed_raw)) == 0) {
+            // check checksum
+            if (!PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) ||
+                !PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) ||
+                !PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2])) {
+// CRC error
+                state->status = SDP3X_STATUS_CONFIGURED;
+                return;
+            }
+
+            state->pressure    = (int16_t)((((uint16_t)airspeed_raw[0]) << 8) + (uint16_t)airspeed_raw[1]);
+            state->temperature = (int16_t)((((uint16_t)airspeed_raw[3]) << 8) + (uint16_t)airspeed_raw[4]);
+            state->scale = (int16_t)((((uint16_t)airspeed_raw[6]) << 8) + (uint16_t)airspeed_raw[7]);
+        } else {
+            state->status = SDP3X_STATUS_NOTFOUND;
+        }
+    }
+}
+
+#endif /* PIOS_INCLUDE_SDP3X */

flight/pios/inc/pios_sdp3x.h

@@ -0,0 +1,62 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors
+ * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3
+ * @{
+ *
+ * @file       pios_sdp3x.h
+ * @author     The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021.
+ * @brief      SDP3x Airspeed Sensor Driver
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ ******************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_SDP3X_H
+#define PIOS_SDP3X_H
+
+#define SDP3X_I2C_ADDR1            0x21
+#define SDP3X_I2C_ADDR2            0x22
+#define SDP3X_I2C_ADDR3            0x23
+
+#define SDP3X_SCALE_TEMPERATURE    200.0f
+#define SDP3X_RESET_ADDR           0x00
+#define SDP3X_RESET_CMD            0x06
+#define SDP3X_CONT_MEAS_AVG_MODE   0x3615
+#define SDP3X_CONT_MODE_STOP       0x3FF9
+
+#define SDP3X_SCALE_PRESSURE_SDP31 60
+#define SDP3X_SCALE_PRESSURE_SDP32 240
+#define SDP3X_SCALE_PRESSURE_SDP33 20
+#define SDP3X_CRC_INIT             0xff
+#define SDP3X_CRC_POLY             0x31
+
+#define SDP3X_CONVERSION_INTERVAL  (1000000 / SDP3X_MEAS_RATE)     /* microseconds */
+
+#define PIOS_SDP3X_RETRY_LIMIT     3
+struct PIOS_SDP3X_STATE {
+    enum { SDP3X_STATUS_NOTFOUND, SDP3X_STATUS_CONFIGURED, SDP3X_STATUS_READY } status;
+    int16_t pressure;
+    int16_t temperature;
+    int16_t scale;
+};
+
+void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state);
+
+#endif /* PIOS_SDP3X_H */

flight/pios/pios.h

@@ -199,6 +199,11 @@ extern "C" {
 #include <pios_etasv3.h>
 #endif
 
+#ifdef PIOS_INCLUDE_SDP3X
+/* SDP3X Airspeed MicroSensor*/
+#include <pios_sdp3x.h>
+#endif
+
 #ifdef PIOS_INCLUDE_MS4525DO
 /* PixHawk Airspeed Sensor based on MS4525DO */
 #include <pios_ms4525do.h>

flight/targets/boards/discoveryf4bare/pios_board.h

@@ -112,6 +112,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id;
 extern uint32_t pios_i2c_flexiport_adapter_id;
 #define PIOS_I2C_FLEXI_ADAPTER            (pios_i2c_flexiport_adapter_id)
 #define PIOS_I2C_ETASV3_ADAPTER           (PIOS_I2C_FLEXI_ADAPTER)
+#define PIOS_I2C_SDP3X_ADAPTER            (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_EXTERNAL_ADAPTER         (PIOS_I2C_FLEXI_ADAPTER)
 
 // -------------------------

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -92,6 +92,7 @@
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
 #define PIOS_INCLUDE_ETASV3
+#define PIOS_INCLUDE_SDP3X
 #define PIOS_INCLUDE_MS4525DO
 /* #define PIOS_INCLUDE_HCSR04 */
 

flight/targets/boards/revolution/pios_board.h

@@ -131,6 +131,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id;
 extern uint32_t pios_i2c_flexiport_adapter_id;
 #define PIOS_I2C_FLEXI_ADAPTER            (pios_i2c_flexiport_adapter_id)
 #define PIOS_I2C_ETASV3_ADAPTER           (PIOS_I2C_FLEXI_ADAPTER)
+#define PIOS_I2C_SDP3X_ADAPTER            (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_MS4525DO_ADAPTER         (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_EXTERNAL_ADAPTER         (PIOS_I2C_FLEXI_ADAPTER)
 

flight/targets/boards/revonano/firmware/inc/pios_config.h

@@ -91,6 +91,7 @@
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
 #define PIOS_INCLUDE_ETASV3
+#define PIOS_INCLUDE_SDP3X
 #define PIOS_INCLUDE_MS4525DO
 #define PIOS_INCLUDE_MPU9250
 #define PIOS_MPU9250_ACCEL

flight/targets/boards/revonano/pios_board.h

@@ -127,6 +127,7 @@ extern uint32_t pios_i2c_eeprom_pressure_adapter_id;
 extern uint32_t pios_i2c_flexiport_adapter_id;
 #define PIOS_I2C_FLEXI_ADAPTER           (pios_i2c_flexiport_adapter_id)
 #define PIOS_I2C_ETASV3_ADAPTER          (PIOS_I2C_FLEXI_ADAPTER)
+#define PIOS_I2C_SDP3X_ADAPTER           (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_MS4525DO_ADAPTER        (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_EXTERNAL_ADAPTER        (PIOS_I2C_FLEXI_ADAPTER)
 

flight/targets/boards/revoproto/firmware/inc/pios_config.h

@@ -89,6 +89,7 @@
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
 #define PIOS_INCLUDE_ETASV3
+#define PIOS_INCLUDE_SDP3X
 /* #define PIOS_INCLUDE_HCSR04 */
 
 #define PIOS_SENSOR_RATE 500.0f

flight/targets/boards/revoproto/pios_board.h

@@ -114,6 +114,7 @@ extern uint32_t pios_i2c_pressure_adapter_id;
 extern uint32_t pios_i2c_flexiport_adapter_id;
 #define PIOS_I2C_FLEXI_ADAPTER            (pios_i2c_flexiport_adapter_id)
 #define PIOS_I2C_ETASV3_ADAPTER           (PIOS_I2C_FLEXI_ADAPTER)
+#define PIOS_I2C_SDP3X_ADAPTER            (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_EXTERNAL_ADAPTER         (pios_i2c_flexiport_adapter_id)
 
 

flight/targets/boards/sparky2/firmware/inc/pios_config.h

@@ -91,6 +91,7 @@
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
 #define PIOS_INCLUDE_ETASV3
+#define PIOS_INCLUDE_SDP3X
 #define PIOS_INCLUDE_MS4525DO
 #define PIOS_INCLUDE_MPU9250
 #define PIOS_MPU9250_ACCEL

flight/targets/boards/sparky2/pios_board.h

@@ -133,6 +133,7 @@ extern uint32_t pios_i2c_pressure_adapter_id;
 extern uint32_t pios_i2c_flexiport_adapter_id;
 #define PIOS_I2C_FLEXI_ADAPTER           (pios_i2c_flexiport_adapter_id)
 #define PIOS_I2C_ETASV3_ADAPTER          (PIOS_I2C_FLEXI_ADAPTER)
+#define PIOS_I2C_SDP3X_ADAPTER           (PIOS_I2C_FLEXI_ADAPTER)
 #define PIOS_I2C_MS4525DO_ADAPTER        (PIOS_I2C_FLEXI_ADAPTER)
 
 // -------------------------

shared/uavobjectdefinition/airspeedsettings.xml

@@ -4,7 +4,7 @@
         <field name="SamplePeriod" units="ms" type="uint8" elements="1" defaultvalue="100"/>
         <field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
         <field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
-        <field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None"  defaultvalue="None"/>
+        <field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,SDP3x,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None"  defaultvalue="None"/>
 	    <field name="IMUBasedEstimationLowPassPeriod1" units="s" type="float" elements="1" defaultvalue="0.5" />
         <field name="IMUBasedEstimationLowPassPeriod2" units="s" type="float" elements="1" defaultvalue="10" />
         <access gcs="readwrite" flight="readwrite"/>