airspeed sensor driver

Makefile

@@ -81,6 +81,7 @@ NAZE_SRC	 = drv_adc.c \
 		   drv_ak8975.c \
 		   drv_bma280.c \
 		   drv_bmp085.c \
+		   drv_ms4525.c \
 		   drv_ms5611.c \
 		   drv_bmp280.c \
 		   drv_hcsr04.c \

src/board.h

@@ -60,6 +60,7 @@ typedef enum {
     SENSOR_SONAR = 1 << 4,
     SENSOR_GPS = 1 << 5,
     SENSOR_GPSMAG = 1 << 6,
+    SENSOR_AIRSPEED = 1 << 7,
 } AvailableSensors;
 
 // Type of accelerometer used/detected
@@ -223,6 +224,7 @@ typedef struct baro_t {
 #define GPS
 #define LEDRING
 #define SONAR
+#define AIRSPEED
 #define BUZZER
 #define LED0
 #define LED1
@@ -243,6 +245,7 @@ typedef struct baro_t {
 #include "drv_bma280.h"
 #include "drv_bmp085.h"
 #include "drv_bmp280.h"
+#include "drv_ms4525.h"
 #include "drv_ms5611.h"
 #include "drv_hmc5883l.h"
 #include "drv_ak8975.h"

src/cli.c

@@ -70,7 +70,7 @@ static const char *const featureNames[] = {
 
 // sync this with AvailableSensors enum from board.h
 static const char *const sensorNames[] = {
-    "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
+    "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", "AIRSPEED", NULL
 };
 
 // sync this with AccelSensors enum from board.h

src/drv_ms4525.c

@@ -0,0 +1,51 @@
+#include "board.h"
+
+// MS4525, address 0x28 for the most common version
+#define MS4525_ADDR                 0x28
+#define STATUS_MASK                 0x3F
+
+static void ms4525_read(int16_t *airspeedData);
+
+bool ms4525Detect(sensor_t *airspeed)
+{
+    bool ack;
+    uint8_t buf[1];
+
+    ack = i2cRead(MS4525_ADDR, 0xFF, 1, buf);
+
+    if(!ack)
+        return false;
+
+    airspeed->read = ms4525_read;
+    airspeed->scale = 0;
+
+    return true;
+}
+
+static void ms4525_read(int16_t *airspeedData)
+{
+    uint8_t buf[4];
+    int16_t data[2];
+
+    i2cRead(MS4525_ADDR, 0xFF, 4, buf);
+
+    uint8_t status = (buf[0] >> 5); // first two bits are status bits
+    if(status == 0x00) // good data packet
+    {
+        data[0] = (int16_t)(((STATUS_MASK | buf[0]) << 8) | buf[1]);
+        data[1] = (int16_t)((buf[2] << 3) | (buf[3] >> 5));
+    }
+    else if(status == 0x02) // stale data packet
+    {
+        data[0] = (int16_t)(((STATUS_MASK | buf[0]) << 8) | buf[1]);
+        data[1] = (int16_t)((buf[2] << 3) | (buf[3] >> 5));
+    }
+    else
+    {
+        data[0] = 0;
+        data[1] = 0;
+    }
+
+    airspeedData[0] = data[0];
+    airspeedData[1] = data[1];
+}

src/drv_ms4525.h

@@ -0,0 +1,3 @@
+#pragma once
+
+bool ms4525Detect(sensor_t *airspeed);

src/mw.c

@@ -924,7 +924,7 @@ void loop(void)
                 }
 #endif
             case 4:
-                taskOrder = 0;
+                taskOrder++;
 #ifdef SONAR
                 if (sensors(SENSOR_SONAR)) {
                     Sonar_update();
@@ -933,6 +933,14 @@ void loop(void)
                 if (feature(FEATURE_VARIO) && f.VARIO_MODE)
                     mwVario();
                 break;
+            case 5:
+                taskOrder = 0;
+#ifdef AIRSPEED
+                if (sensors(SENSOR_AIRSPEED)) {
+                    Airspeed_update();
+                }
+#endif
+                break;
         }
     }
 

src/mw.h

@@ -477,6 +477,8 @@ extern uint16_t vbat;                  // battery voltage in 0.1V steps
 extern int16_t telemTemperature1;      // gyro sensor temperature
 extern int32_t amperage;               // amperage read by current sensor in 0.01A steps
 extern int32_t mAhdrawn;              // milli ampere hours drawn from battery since start
+extern int16_t airspeedVelocity;
+extern int16_t airspeedTemp;
 
 #define PITCH_LOOKUP_LENGTH 7
 #define THROTTLE_LOOKUP_LENGTH 12
@@ -513,6 +515,7 @@ extern flags_t f;
 extern sensor_t acc;
 extern sensor_t gyro;
 extern baro_t baro;
+extern sensor_t airspeed;
 
 // main
 void setPIDController(int type);
@@ -538,6 +541,7 @@ int Mag_getADC(void);
 void Sonar_init(void);
 void Sonar_update(void);
 uint16_t RSSI_getValue(void);
+void Airspeed_update(void);
 
 // Output
 void mixerInit(void);

src/sensors.c

@@ -12,6 +12,8 @@ uint16_t calibratingB = 0;      // baro calibration = get new ground pressure va
 uint16_t calibratingG = 0;
 uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
 int16_t heading, magHold;
+int16_t airspeedVelocity = 0;       // raw airspeed
+int16_t airspeedTemp = 0;           // raw air temperature
 
 extern uint16_t InflightcalibratingA;
 extern bool AccInflightCalibrationMeasurementDone;
@@ -26,6 +28,7 @@ sensor_t acc;                       // acc access functions
 sensor_t gyro;                      // gyro access functions
 sensor_t mag;                       // mag access functions
 baro_t baro;                        // barometer access functions
+sensor_t airspeed;                  // airspeed access functions
 uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
 uint8_t magHardware = MAG_DEFAULT;
 
@@ -130,6 +133,12 @@ bool sensorsAutodetect(void)
     }
 #endif
 
+#ifdef AIRSPEED
+    sensorsSet(SENSOR_AIRSPEED);
+    if (!ms4525Detect(&airspeed))
+        sensorsClear(SENSOR_AIRSPEED);
+#endif
+
     // Now time to init things, acc first
     if (sensors(SENSOR_ACC))
         acc.init(mcfg.acc_align);
@@ -536,3 +545,28 @@ void Sonar_update(void)
 }
 
 #endif
+
+#ifdef AIRSPEED
+void Airspeed_update(void)
+{
+    static uint32_t lastMeasurement = 0;
+    static int32_t delay = 50000; // ~20 hz
+
+    if (lastMeasurement > currentTime) // protect from overflow of currentTime
+        lastMeasurement = currentTime;
+
+    if ((int32_t)(currentTime - lastMeasurement) < delay)
+        return;
+
+    lastMeasurement = currentTime;
+
+    int16_t airspeedData[2];
+    airspeed.read(airspeedData);
+
+    if(airspeedData[0] != 0)
+    {
+        airspeedVelocity = airspeedData[0];
+        airspeedTemp = airspeedData[1];
+    }
+}
+#endif /* AIRSPEED */

src/serial.c

@@ -40,6 +40,7 @@
 #define MSP_NAV_STATUS           121    //out message         Returns navigation status
 #define MSP_NAV_CONFIG           122    //out message         Returns navigation parameters
 #define MSP_FW_CONFIG            123    //out message         Returns parameters specific to Flying Wing mode
+#define MSP_RAW_AIRSPEED         124    //out message         airspeed, temperature
 
 #define MSP_SET_RAW_RC           200    //in message          8 rc chan
 #define MSP_SET_RAW_GPS          201    //in message          fix, numsat, lat, lon, alt, speed
@@ -610,6 +611,11 @@ static void evaluateCommand(void)
             serialize32(EstAlt);
             serialize16(vario);
             break;
+        case MSP_RAW_AIRSPEED:
+            headSerialReply(4);
+            serialize16(airspeedVelocity);
+            serialize16(airspeedTemp);
+            break;
         case MSP_ANALOG:
             headSerialReply(7);
             serialize8((uint8_t)constrain(vbat, 0, 255));