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