diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index c3455a5529d87..d6d85d58f0a89 100644
--- a/Tools/scripts/build_options.py
+++ b/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),
diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp
index 9994d31f40fc6..9c0783bf83df1 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.cpp
+++ b/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;
}
diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h
index e4f9d760e01f5..19488c7b0b7ea 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.h
+++ b/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,
};
diff --git a/libraries/AP_Airspeed/AP_Airspeed_AUAV.cpp b/libraries/AP_Airspeed/AP_Airspeed_AUAV.cpp
new file mode 100644
index 0000000000000..c5453e8cc29f3
--- /dev/null
+++ b/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 .
+ */
+
+/*
+ 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
+#include
+#include
+#include
+#include
+
+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
diff --git a/libraries/AP_Airspeed/AP_Airspeed_AUAV.h b/libraries/AP_Airspeed/AP_Airspeed_AUAV.h
new file mode 100644
index 0000000000000..41e75a18c6f80
--- /dev/null
+++ b/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 .
+ */
+#pragma once
+
+#include "AP_Airspeed_config.h"
+
+#if AP_AIRSPEED_AUAV_ENABLED
+
+/*
+ backend driver for airspeed from I2C
+ */
+
+#include
+#include
+#include
+#include
+
+#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 _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
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
index bdb4dded5c562..3eb54c07ad7f2 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
@@ -127,6 +127,7 @@ class AP_Airspeed_Backend {
ANALOG = 0x08,
NMEA = 0x09,
ASP5033 = 0x0A,
+ AUAV = 0x0B,
};
private:
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
index 97403e88b1358..0ddeda5553739 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
+++ b/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),
diff --git a/libraries/AP_Airspeed/AP_Airspeed_config.h b/libraries/AP_Airspeed/AP_Airspeed_config.h
index f12db5cb16c4e..9e2c46e690837 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_config.h
+++ b/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