AUAV Airspeed Sensor Driver

Tools/scripts/build_options.py

@@ -407,6 +407,7 @@ def config_option(self):
     Feature('Airspeed Drivers', 'NMEA_AIRSPEED', 'AP_AIRSPEED_NMEA_ENABLED', 'Enable NMEA AIRSPEED', 0, 'AIRSPEED'),
     Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'),
     Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'),   # NOQA: E501
+    Feature('Airspeed Drivers', 'AUAV_AIRSPEED', 'AP_AIRSPEED_AUAV_ENABLED', 'ENABLE AUAV AIRSPEED', 0, 'AIRSPEED'),
 
     Feature('Actuators', 'ServoTelem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None),
     Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None),

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -52,6 +52,7 @@
 #include "AP_Airspeed_DroneCAN.h"
 #include "AP_Airspeed_NMEA.h"
 #include "AP_Airspeed_MSP.h"
+#include "AP_Airspeed_AUAV.h"
 #include "AP_Airspeed_External.h"
 #include "AP_Airspeed_SITL.h"
 extern const AP_HAL::HAL &hal;
@@ -439,6 +440,21 @@ void AP_Airspeed::allocate()
         case TYPE_EXTERNAL:
 #if AP_AIRSPEED_EXTERNAL_ENABLED
             sensor[i] = NEW_NOTHROW AP_Airspeed_External(*this, i);
+#endif
+            break;
+        case TYPE_AUAV_5IN:
+#if AP_AIRSPEED_AUAV_ENABLED
+            sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 5);
+#endif
+            break;
+        case TYPE_AUAV_10IN:
+#if AP_AIRSPEED_AUAV_ENABLED
+            sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 10);
+#endif
+            break;
+        case TYPE_AUAV_30IN:
+#if AP_AIRSPEED_AUAV_ENABLED
+            sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 30);
 #endif
             break;
         }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -192,6 +192,9 @@ class AP_Airspeed
         TYPE_MSP=14,
         TYPE_I2C_ASP5033=15,
         TYPE_EXTERNAL=16,
+        TYPE_AUAV_10IN=17,
+        TYPE_AUAV_5IN=18,
+        TYPE_AUAV_30IN=19,
         TYPE_SITL=100,
     };
 

libraries/AP_Airspeed/AP_Airspeed_AUAV.cpp

@@ -0,0 +1,246 @@
+/*
+   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, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver for airspeed from a I2C AUAV sensor
+ */
+#include "AP_Airspeed_AUAV.h"
+
+#if AP_AIRSPEED_AUAV_ENABLED
+
+#define AUAV_AIRSPEED_I2C_ADDR 0x26
+
+// the sensor supports a 2nd channel (2nd I2C address) for absolute pressure
+// we could use this as a baro driver but don't yet
+#define AUAV_AIRSPEED_I2C_ADDR_ABSOLUTE 0x27
+
+// max measurement time for 8x averaging differential is 16.2ms
+#define MEASUREMENT_TIME_MAX_MS 17
+
+#define MEASUREMENT_TIMEOUT_MS 200
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL &hal;
+
+
+AP_Airspeed_AUAV::AP_Airspeed_AUAV(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) :
+    AP_Airspeed_Backend(_frontend, _instance),
+    range_inH2O(_range_inH2O)
+{
+}
+
+// probe for a sensor
+bool AP_Airspeed_AUAV::probe(uint8_t bus, uint8_t address)
+{
+    _dev = hal.i2c_mgr->get_device(bus, address);
+    if (!_dev) {
+        return false;
+    }
+
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    _dev->set_retries(10);
+    _measure();
+    hal.scheduler->delay(10);
+    _collect();
+
+    return last_sample_time_ms != 0;
+
+}
+
+// probe and initialise the sensor
+bool AP_Airspeed_AUAV::init()
+{
+    if (bus_is_configured()) {
+        // the user has configured a specific bus
+        if (probe(get_bus(), AUAV_AIRSPEED_I2C_ADDR)) {
+            goto found_sensor;
+        }
+    } else {
+        // if bus is not configured then fall back to the old
+        // behaviour of probing all buses, external first
+        FOREACH_I2C_EXTERNAL(bus) {
+            if (probe(bus, AUAV_AIRSPEED_I2C_ADDR)) {
+                goto found_sensor;
+            }
+        }
+        FOREACH_I2C_INTERNAL(bus) {
+            if (probe(bus, AUAV_AIRSPEED_I2C_ADDR)) {
+                goto found_sensor;
+            }
+        }
+    }
+
+    GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AUAV AIRSPEED[%u]: no sensor found", get_instance());
+    return false;
+
+found_sensor:
+    _dev->set_device_type(uint8_t(DevType::AUAV));
+    set_bus_id(_dev->get_bus_id());
+
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AUAV AIRSPEED[%u]: Found bus %u addr 0x%02x", get_instance(), _dev->bus_num(), _dev->get_bus_address());
+
+    // drop to 2 retries for runtime
+    _read_coefficients();
+    _dev->set_retries(2);
+    _dev->register_periodic_callback(20000,
+                                     FUNCTOR_BIND_MEMBER(&AP_Airspeed_AUAV::_timer, void));
+    return true;
+}
+
+// start a measurement
+void AP_Airspeed_AUAV::_measure()
+{
+    measurement_started_ms = 0;
+    uint8_t cmd = 0xAD; // start average8, max 16.2ms measurement time
+    if (_dev->transfer(&cmd, 1, nullptr, 0)) {
+        measurement_started_ms = AP_HAL::millis();
+    }
+}
+
+// read the values from the sensor
+void AP_Airspeed_AUAV::_collect()
+{
+    measurement_started_ms = 0; // It should always get reset by _measure. This is a safety to handle failures of i2c bus
+    uint8_t inbuf[7];
+    if (!_dev->read((uint8_t *)&inbuf, sizeof(inbuf))) {
+        return;
+    }
+    const int32_t Tref_Counts = 7576807; // temperature counts at 25C
+    const float TC50Scale = 100.0f * 100.0f * 167772.2f; // scale TC50 to 1.0% FS0
+    float AP3, BP2, CP, Corr, Pcorr, Pdiff, TC50, Pnfso, Tcorr, PCorrt;
+    int32_t iPraw, Tdiff, iTemp;
+    uint32_t PComp;
+
+    // Convert unsigned 24-bit pressure value to signed +/- 23-bit:
+    iPraw = (inbuf[1]<<16) + (inbuf[2]<<8) + inbuf[3] - 0x800000;
+
+    // Convert signed 23-bit valu11e to float, normalized to +/- 1.0:
+    const float Pnorm = float(iPraw) / float(0x7FFFFF);
+    AP3 = DLIN_A * Pnorm * Pnorm * Pnorm; // A*Pout^3
+    BP2 = DLIN_B * Pnorm * Pnorm; // B*Pout^2
+    CP = DLIN_C * Pnorm; // C*POut
+    Corr = AP3 + BP2 + CP + DLIN_D; // Linearity correction term
+    Pcorr = Pnorm + Corr; // Corrected P, range +/-1.0.
+
+    // Compute difference from reference temperature, in sensor counts:
+    iTemp = (inbuf[4]<<16) + (inbuf[5]<<8) + inbuf[6]; // 24-bit temperature
+
+    // get temperature in degrees C
+    temp_C = (iTemp * 155) / float(1U<<24) - 45;
+
+    Tdiff = iTemp - Tref_Counts; // see constant defined above.
+    Pnfso = (Pcorr + 1.0f) * 0.5;
+
+    //TC50: Select High/Low, based on current temp above/below 25C:
+    if (Tdiff > 0) {
+        TC50 = D_TC50H;
+    } else {
+        TC50 = D_TC50L;
+    }
+
+    // Find absolute difference between midrange and reading (abs(Pnfso-0.5)):
+    if (Pnfso > 0.5) {
+        Pdiff = Pnfso - 0.5;
+    } else {
+        Pdiff = 0.5f - Pnfso;
+    }
+
+    Tcorr = (1.0f - (D_Es * 2.5f * Pdiff)) * Tdiff * TC50 / TC50Scale;
+    PCorrt = Pnfso - Tcorr; // corrected P: float, [0 to +1.0)
+    PComp = (uint32_t) (PCorrt * (float)0xFFFFFF);
+    pressure_digital = PComp;
+
+    last_sample_time_ms = AP_HAL::millis();
+}
+
+uint32_t AP_Airspeed_AUAV::_read_register(uint8_t cmd)
+{
+    uint8_t raw_bytes1[3];
+    if (!_dev->transfer(&cmd,1,(uint8_t *)&raw_bytes1, sizeof(raw_bytes1))) {
+        return 0;
+    }
+    uint8_t raw_bytes2[3];
+    uint8_t cmd2 = cmd + 1;
+    if (!_dev->transfer(&cmd2,1,(uint8_t *)&raw_bytes2, sizeof(raw_bytes2))) {
+        return 0;
+    }
+    uint32_t result = ((uint32_t)raw_bytes1[1] << 24) | ((uint32_t)raw_bytes1[2] << 16) | ((uint32_t)raw_bytes2[1] << 8) | (uint32_t)raw_bytes2[2];
+    return result;
+}
+
+bool AP_Airspeed_AUAV::_read_coefficients()
+{
+    // Differential Coefficients
+    int32_t i32A = 0, i32B =0, i32C =0, i32D=0, i32TC50HLE=0;
+    int8_t i8TC50H = 0, i8TC50L = 0, i8Es = 0;
+    i32A = _read_register(0x2B);
+    DLIN_A = ((float)(i32A))/((float)(0x7FFFFFFF));
+
+    i32B = _read_register(0x2D);
+    DLIN_B = (float)(i32B)/(float)(0x7FFFFFFF);
+
+    i32C = _read_register(0x2F);
+    DLIN_C = (float)(i32C)/(float)(0x7FFFFFFF);
+
+    i32D = _read_register(0x31);
+    DLIN_D = (float)(i32D)/(float)(0x7FFFFFFF);
+
+    i32TC50HLE = _read_register(0x33);
+    i8TC50H = (i32TC50HLE >> 24) & 0xFF; // 55 H
+    i8TC50L = (i32TC50HLE >> 16) & 0xFF; // 55 L
+    i8Es = (i32TC50HLE ) & 0xFF; // 56 L
+    D_Es = (float)(i8Es)/(float)(0x7F);
+    D_TC50H = (float)(i8TC50H)/(float)(0x7F);
+    D_TC50L = (float)(i8TC50L)/(float)(0x7F);
+
+    return true; //Need to actually check
+}
+
+// 50Hz timer
+void AP_Airspeed_AUAV::_timer()
+{
+    if (measurement_started_ms == 0) {
+        _measure();
+    }
+    if ((AP_HAL::millis() - measurement_started_ms) > MEASUREMENT_TIME_MAX_MS) {
+        _collect();
+        // start a new measurement
+        _measure();
+    }
+}
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_AUAV::get_differential_pressure(float &_pressure)
+{
+    WITH_SEMAPHORE(sem);
+    _pressure = 248.8f*1.25f*((pressure_digital-8388608)/16777216.0f) * 2 * range_inH2O;
+    return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS;
+}
+
+// return the current temperature in degrees C, if available
+bool AP_Airspeed_AUAV::get_temperature(float &_temperature)
+{
+    WITH_SEMAPHORE(sem);
+    _temperature = temp_C;
+    return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS;
+}
+
+#endif  // AP_AIRSPEED_AUAV_ENABLED

libraries/AP_Airspeed/AP_Airspeed_AUAV.h

@@ -0,0 +1,73 @@
+/*
+   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, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "AP_Airspeed_config.h"
+
+#if AP_AIRSPEED_AUAV_ENABLED
+
+/*
+  backend driver for airspeed from I2C
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL/I2CDevice.h>
+#include <utility>
+
+#include "AP_Airspeed_Backend.h"
+
+class AP_Airspeed_AUAV : public AP_Airspeed_Backend
+{
+public:
+    AP_Airspeed_AUAV(AP_Airspeed &frontend, uint8_t _instance, const float _range_inH2O);
+    ~AP_Airspeed_AUAV(void) {}
+
+    // probe and initialise the sensor
+    bool init() override;
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure) override;
+
+    // return the current temperature in degrees C, if available
+    bool get_temperature(float &temperature) override;
+
+private:
+    bool probe(uint8_t bus, uint8_t address);
+    void _measure();
+    void _collect();
+    void _timer();
+    bool _read_coefficients();
+    uint32_t _read_register(uint8_t cmd);
+
+    uint32_t last_sample_time_ms;
+    uint32_t measurement_started_ms;
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+
+    float DLIN_A;
+    float DLIN_B;
+    float DLIN_C;
+    float DLIN_D;
+    float D_Es;
+    float D_TC50H;
+    float D_TC50L; // Diff coeffs
+
+    float pressure_digital;
+    float pressure_abs_L;
+    float temp_C;
+    const float range_inH2O;
+};
+
+#endif  // AP_Airspeed_AUAV_ENABLED

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -127,6 +127,7 @@ class AP_Airspeed_Backend {
         ANALOG   = 0x08,
         NMEA     = 0x09,
         ASP5033  = 0x0A,
+        AUAV     = 0x0B,
     };
 
 private:

libraries/AP_Airspeed/AP_Airspeed_Params.cpp

@@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,17:AUAV-10in,18:AUAV-5in,19:AUAV-30in,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_Airspeed/AP_Airspeed_config.h

@@ -55,6 +55,10 @@
 #define AP_AIRSPEED_SITL_ENABLED AP_AIRSPEED_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED
 #endif
 
+#ifndef AP_AIRSPEED_AUAV_ENABLED
+#define AP_AIRSPEED_AUAV_ENABLED AP_AIRSPEED_BACKEND_DEFAULT_ENABLED
+#endif
+
 // other AP_Airspeed options:
 #ifndef AIRSPEED_MAX_SENSORS
 #define AIRSPEED_MAX_SENSORS 2