diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 481d029770..296e9815ad 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -27,7 +27,9 @@ #include "AP_Airspeed.h" #include "AP_Airspeed_MS4525.h" #include "AP_Airspeed_MS5525.h" +#include "AP_Airspeed_SDP3X.h" #include "AP_Airspeed_analog.h" +#include "AP_Airspeed_Backend.h" extern const AP_HAL::HAL &hal; @@ -53,7 +55,7 @@ const AP_Param::GroupInfo AP_Airspeed::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) + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 0, AP_Airspeed, _type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), @@ -112,7 +114,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: BUS // @DisplayName: Airspeed I2C bus // @Description: The bus number of the I2C bus to look for the sensor on - // @Values: 0:Bus0,1:Bus1 + // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) // @User: Advanced AP_GROUPINFO("BUS", 9, AP_Airspeed, _bus, 1), @@ -166,6 +168,9 @@ void AP_Airspeed::init() case TYPE_I2C_MS5525_ADDRESS_2: sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_2); break; + case TYPE_I2C_SDP3X: + sensor = new AP_Airspeed_SDP3X(*this); + break; } if (sensor && !sensor->init()) { gcs().send_text(MAV_SEVERITY_INFO, "Airspeed init failed"); diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 9a3f3ce960..439780c20f 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -4,8 +4,9 @@ #include #include #include +#include -#include "AP_Airspeed_Backend.h" +class AP_Airspeed_Backend; class Airspeed_Calibration { public: @@ -125,7 +126,7 @@ public: void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground); // return health status of sensor - bool healthy(void) const { return _healthy && fabsf(_offset) > 0 && enabled(); } + bool healthy(void) const { return _healthy && (fabsf(_offset) > 0 || _allow_zero_offset) && enabled(); } void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; } @@ -147,6 +148,7 @@ public: TYPE_I2C_MS5525=3, TYPE_I2C_MS5525_ADDRESS_1=4, TYPE_I2C_MS5525_ADDRESS_2=5, + TYPE_I2C_SDP3X=6, }; private: @@ -187,4 +189,7 @@ private: void update_calibration(float raw_pressure); AP_Airspeed_Backend *sensor; + + // some sensors can have zero offset and be healthy + bool _allow_zero_offset; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp index e1043075bb..44292f633b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp @@ -20,6 +20,7 @@ #include #include #include "AP_Airspeed.h" +#include "AP_Airspeed_Backend.h" extern const AP_HAL::HAL &hal; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index bc59c2a6e2..e4197a8c87 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -20,8 +20,7 @@ #include #include - -class AP_Airspeed; +#include "AP_Airspeed.h" class AP_Airspeed_Backend { public: @@ -42,8 +41,31 @@ protected: float get_psi_range(void) const; uint8_t get_bus(void) const; + AP_Airspeed::pitot_tube_order get_tube_order(void) const { + return AP_Airspeed::pitot_tube_order(frontend._tube_order.get()); + } + // semaphore for access to shared frontend data - AP_HAL::Semaphore *sem; + AP_HAL::Semaphore *sem; + + float get_airspeed_ratio(void) const { + return frontend.get_airspeed_ratio(); + } + + // some sensors allow zero offsets while healthy + void set_allow_zero_offset(void) { + frontend._allow_zero_offset = true; + } + + // set to no zero cal, which makes sense for some sensors + void set_skip_cal(void) { + frontend._skip_cal.set(1); + } + + // set zero offset + void set_offset(float ofs) { + frontend._offset.set(ofs); + } private: AP_Airspeed &frontend; diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp new file mode 100644 index 0000000000..535b6836dc --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp @@ -0,0 +1,308 @@ +/* + 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 SDP3X sensor + + with thanks to https://github.com/PX4/Firmware/blob/master/src/drivers/sdp3x_airspeed + */ +#include "AP_Airspeed_SDP3X.h" + +#include + +#define SDP3X_SCALE_TEMPERATURE 200.0f + +#define SDP3XD0_I2C_ADDR 0x21 +#define SDP3XD1_I2C_ADDR 0x22 +#define SDP3XD2_I2C_ADDR 0x23 + +#define SDP3X_CONT_MEAS_AVG_MODE 0x3615 +#define SDP3X_CONT_MEAS_STOP 0x3FF9 + +#define SDP3X_SCALE_PRESSURE_SDP31 60 +#define SDP3X_SCALE_PRESSURE_SDP32 240 +#define SDP3X_SCALE_PRESSURE_SDP33 20 + +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ + +extern const AP_HAL::HAL &hal; + +AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend) : + AP_Airspeed_Backend(_frontend) +{ +} + +/* + send a 16 bit command code + */ +bool AP_Airspeed_SDP3X::_send_command(uint16_t cmd) +{ + uint8_t b[2] {uint8_t(cmd >> 8), uint8_t(cmd & 0xFF)}; + return _dev->transfer(b, 2, nullptr, 0); +} + +// probe and initialise the sensor +bool AP_Airspeed_SDP3X::init() +{ + const uint8_t addresses[3] = { SDP3XD0_I2C_ADDR, + SDP3XD1_I2C_ADDR, + SDP3XD2_I2C_ADDR + }; + bool found = false; + bool ret = false; + + for (uint8_t i=0; iget_device(get_bus(), addresses[i]); + if (!_dev) { + continue; + } + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + continue; + } + + // lots of retries during probe + _dev->set_retries(10); + + // stop any precending measurements + if (!_send_command(SDP3X_CONT_MEAS_STOP)) { + _dev->get_semaphore()->give(); + continue; + } + + // start continuous average mode + if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) { + _dev->get_semaphore()->give(); + continue; + } + + hal.scheduler->delay_microseconds(20000); + + // step 3 - get scale + uint8_t val[9]; + ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val)); + if (!ret) { + _dev->get_semaphore()->give(); + continue; + } + + // Check the CRC + if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5]) || !_crc(&val[6], 2, val[8])) { + _dev->get_semaphore()->give(); + continue; + } + + _scale = (((uint16_t)val[6]) << 8) | val[7]; + + _dev->get_semaphore()->give(); + + found = true; + + char c = 'X'; + switch (_scale) { + case SDP3X_SCALE_PRESSURE_SDP31: + c = '1'; + break; + case SDP3X_SCALE_PRESSURE_SDP32: + c = '2'; + break; + case SDP3X_SCALE_PRESSURE_SDP33: + c = '3'; + break; + } + hal.console->printf("SDP3%c: Found on bus %u address 0x%02x scale=%u\n", + c, get_bus(), addresses[i], _scale); + } + + if (!found) { + return false; + } + + /* + this sensor uses zero offset and skips cal + */ + set_allow_zero_offset(); + set_skip_cal(); + set_offset(0); + + // drop to 2 retries for runtime + _dev->set_retries(2); + + _dev->register_periodic_callback(20000, + FUNCTOR_BIND_MEMBER(&AP_Airspeed_SDP3X::_timer, void)); + return true; +} + +// read the values from the sensor. Called at 50Hz +void AP_Airspeed_SDP3X::_timer() +{ + // read 6 bytes from the sensor + uint8_t val[6]; + int ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val)); + uint32_t now = AP_HAL::millis(); + if (!ret) { + if (now - _last_sample_time_ms > 200) { + // try and re-connect + _send_command(SDP3X_CONT_MEAS_AVG_MODE); + } + return; + } + // Check the CRC + if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5])) { + return; + } + + int16_t P = (((int16_t)val[0]) << 8) | val[1]; + int16_t temp = (((int16_t)val[3]) << 8) | val[4]; + + float P_float = (float)P; + float temp_float = (float)temp; + + float scale_float = (float)_scale; + + float diff_press_pa = P_float / scale_float; + float temperature = temp_float / (float)SDP3X_SCALE_TEMPERATURE; + + if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _press_sum += diff_press_pa; + _temp_sum += temperature; + _press_count++; + _temp_count++; + _last_sample_time_ms = now; + sem->give(); + } +} + +/* + correct pressure for barometric height + With thanks to: + https://github.com/PX4/Firmware/blob/master/Tools/models/sdp3x_pitot_model.py + */ +float AP_Airspeed_SDP3X::_correct_pressure(float press) +{ + const float tube_len = 0.2; + float temperature; + AP_Baro *baro = AP_Baro::get_instance(); + + if (baro == nullptr) { + return press; + } + + // fix for tube order + AP_Airspeed::pitot_tube_order tube_order = get_tube_order(); + switch (tube_order) { + case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE: + press = -press; + //FALLTHROUGH; + case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE: + break; + case AP_Airspeed::PITOT_TUBE_ORDER_AUTO: + default: + press = fabsf(press); + break; + } + if (press <= 0) { + return 0; + } + + get_temperature(temperature); + float rho_air = baro->get_pressure() / (CONSTANTS_AIR_GAS_CONST * (temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + + // flow through sensor + float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1)) * 1.29f / rho_air; + if (flow_SDP3X < 0.0f) { + flow_SDP3X = 0.0f; + } + + // diffential pressure through pitot tube + float dp_pitot = 28557670.0f - 28557670.0f / (1 + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)); + + // pressure drop through tube + float dp_tube = flow_SDP3X * 0.000746124f * tube_len * rho_air; + + // uncorrected pressure + float press_uncorrected = (press + dp_tube + dp_pitot) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + + // correction for speed at pitot-tube tip due to flow through sensor + float dv = 0.0331582 * flow_SDP3X; + + // airspeed ratio + float ratio = get_airspeed_ratio(); + + // calculate equivalent pressure correction + float press_correction = sq(sqrtf(press_uncorrected*ratio)+dv)/ratio - press_uncorrected; + + return press_uncorrected + press_correction; +} + +// return the current differential_pressure in Pascal +bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure) +{ + uint32_t now = AP_HAL::millis(); + if (now - _last_sample_time_ms > 100) { + return false; + } + if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (_press_count > 0) { + _press = _press_sum / _press_count; + _press_count = 0; + _press_sum = 0; + } + sem->give(); + } + pressure = _correct_pressure(_press); + return true; +} + +// return the current temperature in degrees C, if available +bool AP_Airspeed_SDP3X::get_temperature(float &temperature) +{ + if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { + return false; + } + if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (_temp_count > 0) { + _temp = _temp_sum / _temp_count; + _temp_count = 0; + _temp_sum = 0; + } + sem->give(); + } + temperature = _temp; + return true; +} + +/* + check CRC for a set of bytes + */ +bool AP_Airspeed_SDP3X::_crc(const uint8_t data[], unsigned size, uint8_t checksum) +{ + uint8_t crc_value = 0xff; + + // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1) + for (uint8_t i = 0; i < size; i++) { + crc_value ^= data[i]; + for (uint8_t bit = 8; bit > 0; --bit) { + if (crc_value & 0x80) { + crc_value = (crc_value << 1) ^ 0x31; + } else { + crc_value = (crc_value << 1); + } + } + } + // verify checksum + return (crc_value == checksum); +} diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h new file mode 100644 index 0000000000..cb756b63a3 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h @@ -0,0 +1,70 @@ +/* + 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 + +/* + backend driver for airspeed from I2C + */ + +#include +#include +#include +#include +#include + +#include "AP_Airspeed_Backend.h" +#include "AP_Airspeed.h" + +/* + * Thanks to PX4 for registers definitions + */ + +class AP_Airspeed_SDP3X : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_SDP3X(AP_Airspeed &frontend); + ~AP_Airspeed_SDP3X(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: + void _collect(); + void _timer(); + bool _send_command(uint16_t cmd); + float _get_pressure(int16_t dp_raw) const; + float _get_temperature(int16_t dT_raw) const; + bool _crc(const uint8_t data[], unsigned size, uint8_t checksum); + float _correct_pressure(float press); + + float _temp; + float _press; + float _temperature; + float _pressure; + uint16_t _temp_count; + uint16_t _press_count; + float _temp_sum; + float _press_sum; + uint32_t _last_sample_time_ms; + uint16_t _scale; + + AP_HAL::OwnPtr _dev; +};