diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 01032d3355..230b81d884 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -25,6 +25,7 @@ #include #include "AP_Airspeed.h" #include "AP_Airspeed_MS4525.h" +#include "AP_Airspeed_MS5525.h" #include "AP_Airspeed_analog.h" extern const AP_HAL::HAL &hal; @@ -51,7 +52,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 + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525 // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 0, AP_Airspeed, _type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), @@ -106,6 +107,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device // @User: Advanced AP_GROUPINFO("PSI_RANGE", 8, AP_Airspeed, _psi_range, PSI_RANGE_DEFAULT), + + // @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 + // @User: Advanced + AP_GROUPINFO("BUS", 9, AP_Airspeed, _bus, 1), AP_GROUPEND }; @@ -148,9 +156,14 @@ void AP_Airspeed::init() case TYPE_ANALOG: sensor = new AP_Airspeed_Analog(*this); break; + case TYPE_I2C_MS5525: + sensor = new AP_Airspeed_MS5525(*this); + break; } if (sensor && !sensor->init()) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed init failed"); + delete sensor; + sensor = nullptr; } } @@ -193,8 +206,6 @@ void AP_Airspeed::calibrate(bool in_startup) if (in_startup && _skip_cal) { return; } - // discard first reading - get_pressure(); _cal.start_ms = AP_HAL::millis(); _cal.count = 0; _cal.sum = 0; @@ -209,7 +220,7 @@ void AP_Airspeed::update_calibration(float raw_pressure) // consider calibration complete when we have at least 10 samples // over at least 1 second if (AP_HAL::millis() - _cal.start_ms >= 1000 && - _cal.read_count > 10) { + _cal.read_count > 15) { if (_cal.count == 0) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy"); } else { @@ -219,7 +230,8 @@ void AP_Airspeed::update_calibration(float raw_pressure) _cal.start_ms = 0; return; } - if (_healthy) { + // we discard the first 5 samples + if (_healthy && _cal.read_count > 5) { _cal.sum += raw_pressure; _cal.count++; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index da17f875d2..2289f549da 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -146,6 +146,7 @@ public: TYPE_NONE=0, TYPE_I2C_MS4525=1, TYPE_ANALOG=2, + TYPE_I2C_MS5525=3, }; private: @@ -155,6 +156,7 @@ private: AP_Int8 _use; AP_Int8 _type; AP_Int8 _pin; + AP_Int8 _bus; AP_Int8 _autocal; AP_Int8 _tube_order; AP_Int8 _skip_cal; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp index d2490e3373..e1043075bb 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp @@ -21,9 +21,18 @@ #include #include "AP_Airspeed.h" +extern const AP_HAL::HAL &hal; + AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend) : frontend(_frontend) -{} +{ + sem = hal.util->new_semaphore(); +} + +AP_Airspeed_Backend::~AP_Airspeed_Backend(void) +{ + delete sem; +} int8_t AP_Airspeed_Backend::get_pin(void) const @@ -36,3 +45,7 @@ float AP_Airspeed_Backend::get_psi_range(void) const return frontend._psi_range; } +uint8_t AP_Airspeed_Backend::get_bus(void) const +{ + return frontend._bus; +} diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index eb5ee1d725..bc59c2a6e2 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -26,6 +26,7 @@ class AP_Airspeed; class AP_Airspeed_Backend { public: AP_Airspeed_Backend(AP_Airspeed &frontend); + virtual ~AP_Airspeed_Backend(); // probe and initialise the sensor virtual bool init(void) = 0; @@ -39,6 +40,10 @@ public: protected: int8_t get_pin(void) const; float get_psi_range(void) const; + uint8_t get_bus(void) const; + + // semaphore for access to shared frontend data + AP_HAL::Semaphore *sem; private: AP_Airspeed &frontend; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp index 88f3e36809..4905945814 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp @@ -43,30 +43,49 @@ AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend) : // probe and initialise the sensor bool AP_Airspeed_MS4525::init() { - _dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR); + const struct { + uint8_t bus; + uint8_t addr; + } addresses[] = { + { 1, MS4525D0_I2C_ADDR }, + { 0, MS4525D0_I2C_ADDR }, + }; + bool found = false; + for (uint8_t i=0; iget_device(addresses[i].bus, addresses[i].addr); + if (!_dev) { + continue; + } + if (!_dev->get_semaphore()->take(0)) { + continue; + } - // take i2c bus sempahore - if (!_dev || !_dev->get_semaphore()->take(200)) { + // lots of retries during probe + _dev->set_retries(10); + + _measure(); + hal.scheduler->delay(10); + _collect(); + + _dev->get_semaphore()->give(); + + found = (_last_sample_time_ms != 0); + if (found) { + printf("MS4525: Found sensor on bus %u address 0x%02x\n", addresses[i].bus, addresses[i].addr); + break; + } + } + if (!found) { + printf("MS4525: no sensor found\n"); return false; } - // lots of retries during probe - _dev->set_retries(5); - - _measure(); - hal.scheduler->delay(10); - _collect(); - _dev->get_semaphore()->give(); - // drop to 2 retries for runtime _dev->set_retries(2); - if (_last_sample_time_ms != 0) { - _dev->register_periodic_callback(20000, - FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool)); - return true; - } - return false; + _dev->register_periodic_callback(20000, + FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool)); + return true; } // start a measurement @@ -114,10 +133,18 @@ void AP_Airspeed_MS4525::_collect() */ float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); - _pressure = diff_press_PSI * PSI_to_Pa; - _temperature = ((200.0f * dT_raw) / 2047) - 50; + float press = diff_press_PSI * PSI_to_Pa; + float temp = ((200.0f * dT_raw) / 2047) - 50; + + _voltage_correction(press, temp); - _voltage_correction(_pressure, _temperature); + if (sem->take(0)) { + _press_sum += press; + _temp_sum += temp; + _press_count++; + _temp_count++; + sem->give(); + } _last_sample_time_ms = AP_HAL::millis(); } @@ -166,6 +193,14 @@ bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure) if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { return false; } + if (sem->take(0)) { + if (_press_count > 0) { + _pressure = _press_sum / _press_count; + _press_count = 0; + _press_sum = 0; + } + sem->give(); + } pressure = _pressure; return true; } @@ -176,6 +211,14 @@ bool AP_Airspeed_MS4525::get_temperature(float &temperature) if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { return false; } + if (sem->take(0)) { + if (_temp_count > 0) { + _temperature = _temp_sum / _temp_count; + _temp_count = 0; + _temp_sum = 0; + } + sem->give(); + } temperature = _temperature; return true; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h index de217e5966..b89edcc69e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h @@ -47,7 +47,11 @@ private: void _collect(); bool _timer(); void _voltage_correction(float &diff_press_pa, float &temperature); - + + float _temp_sum; + float _press_sum; + uint32_t _temp_count; + uint32_t _press_count; float _temperature; float _pressure; uint32_t _last_sample_time_ms; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp new file mode 100644 index 0000000000..1c8f8e71b2 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp @@ -0,0 +1,286 @@ +/* + 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 MS5525D0 sensor + */ +#include "AP_Airspeed_MS5525.h" + +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +#define MS5525D0_I2C_ADDR_1 0x76 +#define MS5525D0_I2C_ADDR_2 0x77 + +#define REG_RESET 0x1E +#define REG_CONVERT_D1_OSR_256 0x40 +#define REG_CONVERT_D1_OSR_512 0x42 +#define REG_CONVERT_D1_OSR_1024 0x44 +#define REG_CONVERT_D1_OSR_2048 0x46 +#define REG_CONVERT_D1_OSR_4096 0x48 +#define REG_CONVERT_D2_OSR_256 0x50 +#define REG_CONVERT_D2_OSR_512 0x52 +#define REG_CONVERT_D2_OSR_1024 0x54 +#define REG_CONVERT_D2_OSR_2048 0x56 +#define REG_CONVERT_D2_OSR_4096 0x58 +#define REG_ADC_READ 0x00 +#define REG_PROM_BASE 0xA0 + +// go for 1024 oversampling. This should be fast enough to reduce +// noise but low enough to keep self-heating small +#define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024 +#define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024 + +AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend) : + AP_Airspeed_Backend(_frontend) +{ +} + +// probe and initialise the sensor +bool AP_Airspeed_MS5525::init() +{ + const uint8_t addresses[] = { MS5525D0_I2C_ADDR_1, MS5525D0_I2C_ADDR_2 }; + bool found = false; + for (uint8_t i=0; iget_device(get_bus(), addresses[i]); + if (!dev) { + continue; + } + if (!dev->get_semaphore()->take(0)) { + continue; + } + + // lots of retries during probe + dev->set_retries(5); + + found = read_prom(); + + if (found) { + printf("MS5525: Found sensor on bus %u address 0x%02x\n", get_bus(), addresses[i]); + break; + } + dev->get_semaphore()->give(); + } + if (!found) { + printf("MS5525: no sensor found\n"); + return false; + } + + // Send a command to read temperature first + uint8_t reg = REG_CONVERT_TEMPERATURE; + dev->transfer(®, 1, nullptr, 0); + state = 0; + + // drop to 2 retries for runtime + dev->set_retries(2); + + dev->get_semaphore()->give(); + + // read at 80Hz + dev->register_periodic_callback(1000000UL/80U, + FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS5525::timer, bool)); + return true; +} + + +/** + * CRC used by MS pressure devices + */ +uint16_t AP_Airspeed_MS5525::crc4_prom(void) +{ + uint16_t n_rem = 0; + uint8_t n_bit; + + for (uint8_t cnt = 0; cnt < sizeof(prom); cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((prom[cnt >> 1]) & 0x00FF); + } else { + n_rem ^= (uint8_t)(prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + } else { + n_rem = (n_rem << 1); + } + } + } + + return (n_rem >> 12) & 0xF; +} + +bool AP_Airspeed_MS5525::read_prom(void) +{ + // reset the chip to ensure it has correct prom values loaded + uint8_t reg = REG_RESET; + if (!dev->transfer(®, 1, nullptr, 0)) { + return false; + } + hal.scheduler->delay(5); + + bool all_zero = true; + for (uint8_t i = 0; i < 8; i++) { + if (!dev->read_uint16_be(REG_PROM_BASE+i*2, prom[i])) { + return false; + } + if (prom[i] != 0) { + all_zero = false; + } + } + + if (all_zero) { + return false; + } + + /* save the read crc */ + const uint16_t crc_read = prom[7] & 0xf; + + /* remove CRC byte */ + prom[7] &= 0xff00; + + uint16_t crc_calc = crc4_prom(); + if (crc_read != crc_calc) { + printf("MS5525: CRC mismatch 0x%04x 0x%04x\n", crc_read, crc_calc); + } + return crc_read == crc_calc; +} + + +/* + read from the ADC + */ +int32_t AP_Airspeed_MS5525::read_adc() +{ + uint8_t val[3]; + if (!dev->read_registers(REG_ADC_READ, val, 3)) { + return 0; + } + return (val[0] << 16) | (val[1] << 8) | val[2]; +} + +/* + calculate pressure and temperature + */ +void AP_Airspeed_MS5525::calculate(void) +{ + // table for the 001DS part, 1PSI range + const uint8_t Q1 = 15; + const uint8_t Q2 = 17; + const uint8_t Q3 = 7; + const uint8_t Q4 = 5; + const uint8_t Q5 = 7; + const uint8_t Q6 = 21; + + int64_t dT = D2 - int64_t(prom[5]) * (1UL<take(0)) { + pressure_sum += P_Pa; + temperature_sum += Temp_C; + press_count++; + temp_count++; + last_sample_time_ms = AP_HAL::millis(); + sem->give(); + } +} + +// 50Hz timer +bool AP_Airspeed_MS5525::timer() +{ + uint32_t adc_val = read_adc(); + + /* + * If read fails, re-initiate a read command for current state or we are + * stuck + */ + uint8_t next_state = state; + if (adc_val != 0) { + next_state = (state + 1) % 5; + + if (state == 0) { + D2 = adc_val; + } else { + D1 = adc_val; + calculate(); + } + } + + uint8_t next_cmd = next_state == 0 ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE; + if (!dev->transfer(&next_cmd, 1, nullptr, 0)) { + return true; + } + + state = next_state; + + return true; +} + +// return the current differential_pressure in Pascal +bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure) +{ + if ((AP_HAL::millis() - last_sample_time_ms) > 100) { + return false; + } + if (sem->take(0)) { + if (press_count > 0) { + pressure = pressure_sum / press_count; + press_count = 0; + pressure_sum = 0; + } + sem->give(); + } + _pressure = pressure; + return true; +} + +// return the current temperature in degrees C, if available +bool AP_Airspeed_MS5525::get_temperature(float &_temperature) +{ + if ((AP_HAL::millis() - last_sample_time_ms) > 100) { + return false; + } + if (sem->take(0)) { + if (temp_count > 0) { + temperature = temperature_sum / temp_count; + temp_count = 0; + temperature_sum = 0; + } + sem->give(); + } + _temperature = temperature; + return true; +} diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h new file mode 100644 index 0000000000..c4cdd5c213 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h @@ -0,0 +1,69 @@ +/* + 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 + +class AP_Airspeed_MS5525 : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_MS5525(AP_Airspeed &frontend); + ~AP_Airspeed_MS5525(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 measure(); + void collect(); + bool timer(); + bool read_prom(void); + uint16_t crc4_prom(void); + int32_t read_adc(); + void calculate(); + + float pressure; + float temperature; + float temperature_sum; + float pressure_sum; + uint32_t temp_count; + uint32_t press_count; + + uint32_t last_sample_time_ms; + + uint16_t prom[8]; + uint8_t state; + int32_t D1; + int32_t D2; + + AP_HAL::OwnPtr dev; +};