diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 0b4011c21c..5f57a57095 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -32,6 +32,7 @@ #include "AP_Baro_BMP085.h" #include "AP_Baro_BMP280.h" #include "AP_Baro_HIL.h" +#include "AP_Baro_KellerLD.h" #include "AP_Baro_MS5611.h" #include "AP_Baro_qflight.h" #include "AP_Baro_QURT.h" @@ -485,6 +486,9 @@ void AP_Baro::init(void) #if APM_BUILD_TYPE(APM_BUILD_ArduSub) ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5837_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5837)); + + ADD_BACKEND(AP_Baro_KellerLD::probe(*this, + std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_KELLERLD_I2C_ADDR)))); #else ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR)))); diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.cpp b/libraries/AP_Baro/AP_Baro_KellerLD.cpp new file mode 100644 index 0000000000..a8bbe84986 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_KellerLD.cpp @@ -0,0 +1,248 @@ +/* + 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 . + */ + +#include "AP_Baro_KellerLD.h" + +#include +#include + +#include + +#define KELLER_DEBUG 0 + +#if KELLER_DEBUG +# define Debug(fmt, args ...) do {printf(fmt "\n", ## args);} while(0) +#else +# define Debug(fmt, args ...) +#endif + +extern const AP_HAL::HAL &hal; + +// Measurement range registers +static const uint8_t CMD_PRANGE_MIN_MSB = 0x13; +static const uint8_t CMD_PRANGE_MIN_LSB = 0x14; +static const uint8_t CMD_PRANGE_MAX_MSB = 0x15; +static const uint8_t CMD_PRANGE_MAX_LSB = 0x16; + +// write to this address to start pressure measurement +static const uint8_t CMD_REQUEST_MEASUREMENT = 0xAC; + +AP_Baro_KellerLD::AP_Baro_KellerLD(AP_Baro &baro, AP_HAL::OwnPtr dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +// Look for the device on the bus and see if it responds appropriately +AP_Baro_Backend *AP_Baro_KellerLD::probe(AP_Baro &baro, AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + AP_Baro_KellerLD *sensor = new AP_Baro_KellerLD(baro, std::move(dev)); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +// The hardware does not need to be reset/initialized +// We read out the measurement range to be used in raw value conversions +bool AP_Baro_KellerLD::_init() +{ + if (!_dev) { + return false; + } + + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + AP_HAL::panic("PANIC: AP_Baro_KellerLD: failed to take serial semaphore for init"); + } + + // high retries for init + _dev->set_retries(10); + + bool cal_read_ok = true; + + uint8_t data[3]; + uint16_t ms_word, ls_word; + + // This device has some undocumented finicky quirks and requires delays when reading out the + // measurement range, but for some reason this isn't an issue when requesting measurements. + // This is why we need to split the transfers with delays like this. + // (Using AP_HAL::I2CDevice::set_split_transfers will not work with these sensors) + + // Read out pressure measurement range + cal_read_ok &= _dev->transfer(&CMD_PRANGE_MIN_MSB, 1, nullptr, 0); + hal.scheduler->delay(1); + cal_read_ok &= _dev->transfer(nullptr, 0, &data[0], 3); + hal.scheduler->delay(1); + + ms_word = (data[1] << 8) | data[2]; + Debug("0x13: %d [%d, %d, %d]", ms_word, data[0], data[1], data[2]); + + cal_read_ok &= _dev->transfer(&CMD_PRANGE_MIN_LSB, 1, nullptr, 0); + hal.scheduler->delay(1); + cal_read_ok &= _dev->transfer(nullptr, 0, &data[0], 3); + hal.scheduler->delay(1); + + ls_word = (data[1] << 8) | data[2]; + Debug("0x14: %d [%d, %d, %d]", ls_word, data[0], data[1], data[2]); + + uint32_t cal_data = (ms_word << 16) | ls_word; + memcpy(&_p_min, &cal_data, sizeof(_p_min)); + Debug("data: %d, p_min: %.2f", cal_data, _p_min); + + cal_read_ok &= _dev->transfer(&CMD_PRANGE_MAX_MSB, 1, nullptr, 0); + hal.scheduler->delay(1); + cal_read_ok &= _dev->transfer(nullptr, 0, &data[0], 3); + hal.scheduler->delay(1); + + ms_word = (data[1] << 8) | data[2]; + Debug("0x15: %d [%d, %d, %d]", ms_word, data[0], data[1], data[2]); + + cal_read_ok &= _dev->transfer(&CMD_PRANGE_MAX_LSB, 1, nullptr, 0); + hal.scheduler->delay(1); + cal_read_ok &= _dev->transfer(nullptr, 0, &data[0], 3); + hal.scheduler->delay(1); + + ls_word = (data[1] << 8) | data[2]; + Debug("0x16: %d [%d, %d, %d]", ls_word, data[0], data[1], data[2]); + + cal_data = (ms_word << 16) | ls_word; + memcpy(&_p_max, &cal_data, sizeof(_p_max)); + Debug("data: %d, p_max: %.2f", cal_data, _p_max); + + cal_read_ok &= !isnan(_p_min) && !isinf(_p_min) && !isnan(_p_max) && !isinf(_p_max); + + cal_read_ok &= _p_max > _p_min; + + if (!cal_read_ok) { + printf("Cal read bad!\n"); + _dev->get_semaphore()->give(); + return false; + } + + const char* name = "Keller LD"; + printf("%s found on bus %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_address()); + + // Send a command to read temperature first + _dev->transfer(&CMD_REQUEST_MEASUREMENT, 1, nullptr, 0); + + memset(&_accum, 0, sizeof(_accum)); + + _instance = _frontend.register_sensor(); + + _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER); + + // lower retries for run + _dev->set_retries(3); + + _dev->get_semaphore()->give(); + + /* Request 100Hz update */ + _dev->register_periodic_callback(10 * AP_USEC_PER_MSEC, + FUNCTOR_BIND_MEMBER(&AP_Baro_KellerLD::_timer, void)); + return true; +} + +// Read out most recent measurement from sensor hw +bool AP_Baro_KellerLD::_read() +{ + uint8_t data[5]; + if (!_dev->transfer(0x0, 1, data, sizeof(data))) { + return false; + Debug("Keller LD read failed!"); + } + + //uint8_t status = data[0]; + uint16_t pressure_raw = (data[1] << 8) | data[2]; + uint16_t temperature_raw = (data[3] << 8) | data[4]; + +#if KELLER_DEBUG + static uint8_t samples = 0; + if (samples < 3) { + samples++; + Debug("data: [%d, %d, %d, %d, %d]", data[0], data[1], data[2], data[3], data[4]); + Debug("pressure_raw: %d\ttemperature_raw: %d", pressure_raw, temperature_raw); + } +#endif + + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128); + _sem->give(); + return true; + } + + return false; +} + +// Periodic callback, regular update at 100Hz +// Read out most recent measurement, and request another +// Max conversion time according to datasheet is ~8ms, so +// max update rate is ~125Hz +void AP_Baro_KellerLD::_timer(void) +{ + _read(); + _dev->transfer(&CMD_REQUEST_MEASUREMENT, 1, nullptr, 0); +} + +// Accumulate a reading, shrink if necessary to prevent overflow +void AP_Baro_KellerLD::_update_and_wrap_accumulator(uint16_t pressure, uint16_t temperature, uint8_t max_count) +{ + _accum.sum_pressure += pressure; + _accum.sum_temperature += temperature; + _accum.num_samples += 1; + + if (_accum.num_samples == max_count) { + _accum.sum_pressure /= 2; + _accum.sum_temperature /= 2; + _accum.num_samples /= 2; + } +} + +// Take the average of accumulated values and push to frontend +void AP_Baro_KellerLD::update() +{ + float sum_pressure, sum_temperature; + float num_samples; + + if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return; + } + + if (_accum.num_samples == 0) { + _sem->give(); + return; + } + + sum_pressure = _accum.sum_pressure; + sum_temperature = _accum.sum_temperature; + num_samples = _accum.num_samples; + memset(&_accum, 0, sizeof(_accum)); + + _sem->give(); + + uint16_t raw_pressure_avg = ((float)sum_pressure) / num_samples; + uint16_t raw_temperature_avg = ((float)sum_temperature) / num_samples; + + // per datasheet + float pressure = (raw_pressure_avg - 16384) * (_p_max - _p_min) / 32768 + _p_min; + pressure *= 100000; // bar -> Pascal + pressure += 101300; // MSL pressure offset + float temperature = ((raw_temperature_avg >> 4) - 24) * 0.05f - 50; + + _copy_to_frontend(_instance, pressure, temperature); +} diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.h b/libraries/AP_Baro/AP_Baro_KellerLD.h new file mode 100644 index 0000000000..1519d32b9a --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_KellerLD.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 . + + + * Driver for 4 LD ... 9 LD line of pressure transducers from Keller: + * http://www.keller-druck.com/home_e/paprod_e/4ld_e.asp + * + * These sensors operate on I2C and come in a variety of form factors. + * The measurement range is between 0-200 bar depbending on the model. + * They are definitely not the worlds smallest pressure transmitter. + * + * Default address is 0x40. + */ + +#pragma once + +#include "AP_Baro_Backend.h" + +#include +#include +#include + +#ifndef HAL_BARO_KELLERLD_I2C_ADDR +#define HAL_BARO_KELLERLD_I2C_ADDR 0x40 +#endif + +class AP_Baro_KellerLD : public AP_Baro_Backend +{ +public: + void update(); + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + AP_Baro_KellerLD(AP_Baro &baro, AP_HAL::OwnPtr dev); + virtual ~AP_Baro_KellerLD(void) {}; + + bool _init(); + + void _timer(); + + bool _read(); + + void _update_and_wrap_accumulator(uint16_t pressure, uint16_t temperature, uint8_t max_count); + + AP_HAL::OwnPtr _dev; + + /* Shared values between thread sampling the HW and main thread */ + /* These are raw outputs, not calculated values */ + struct { + uint32_t sum_pressure; + uint32_t sum_temperature; + uint8_t num_samples; + } _accum; + + uint8_t _instance; + + // measurement range parameters used in pressure calculation + // varies based on model, stored in ROM on device + float _p_min; + float _p_max; +};