From fc1be6e0b1211023acafc5d26e218d7b5594b297 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 24 Jun 2020 07:14:17 +0200 Subject: [PATCH] sdp3x: add '-k' option to keep retrying even if probing fails we have a setup where the sensor is connected via magnets, so the system could boot without sensor initially attached. --- .../differential_pressure/sdp3x/SDP3X.cpp | 50 +++++++++++++------ .../differential_pressure/sdp3x/SDP3X.hpp | 9 +++- .../sdp3x/SDP3X_main.cpp | 15 +++++- 3 files changed, 55 insertions(+), 19 deletions(-) diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 1c8d7c3259..b7efab57da 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -40,10 +40,19 @@ #include "SDP3X.hpp" +using namespace time_literals; + int SDP3X::probe() { - return !init_sdp3x(); + _require_initialization = !init_sdp3x(); + + if (_require_initialization && _keep_retrying) { + PX4_INFO("no sensor found, but will keep retrying"); + return 0; + } + + return _require_initialization ? -1 : 0; } int SDP3X::write_command(uint16_t command) @@ -57,14 +66,12 @@ int SDP3X::write_command(uint16_t command) bool SDP3X::init_sdp3x() { - int ret; - if (get_device_address() == I2C_ADDRESS_1_SDP3X) { // since we are broadcasting, only do it for the first device address - // step 1 - reset on broadcast + // reset on broadcast uint16_t prev_addr = get_device_address(); set_device_address(SDP3X_RESET_ADDR); uint8_t reset_cmd = SDP3X_RESET_CMD; - ret = transfer(&reset_cmd, 1, nullptr, 0); + int ret = transfer(&reset_cmd, 1, nullptr, 0); set_device_address(prev_addr); if (ret != PX4_OK) { @@ -76,31 +83,36 @@ SDP3X::init_sdp3x() px4_usleep(20000); } - // step 2 - configure - ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); + return configure() == 0; +} + +int +SDP3X::configure() +{ + int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); if (ret != PX4_OK) { perf_count(_comms_errors); DEVICE_DEBUG("config failed"); - return false; + return ret; } px4_usleep(10000); - // step 3 - get scale + // get scale uint8_t val[9]; ret = transfer(nullptr, 0, &val[0], sizeof(val)); if (ret != PX4_OK) { perf_count(_comms_errors); PX4_ERR("get scale failed"); - return false; + return ret; } // Check the CRC if (!crc(&val[0], 2, val[2]) || !crc(&val[3], 2, val[5]) || !crc(&val[6], 2, val[8])) { perf_count(_comms_errors); - return false; + return PX4_ERROR; } _scale = (((uint16_t)val[6]) << 8) | val[7]; @@ -119,7 +131,7 @@ SDP3X::init_sdp3x() break; } - return true; + return PX4_OK; } int @@ -173,10 +185,18 @@ SDP3X::collect() void SDP3X::RunImpl() { - int ret = PX4_ERROR; + if (_require_initialization) { + if (configure() == PX4_OK) { + _require_initialization = false; - // measurement phase - ret = collect(); + } else { + // periodically retry to configure + ScheduleDelayed(300_ms); + return; + } + } + + int ret = collect(); uint32_t delay_us = CONVERSION_INTERVAL; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 769a27bd27..23b9be7c63 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -69,9 +69,11 @@ class SDP3X : public Airspeed, public I2CSPIDriver { public: - SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X) : + SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X, + bool keep_retrying = false) : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), + _keep_retrying{keep_retrying} { } @@ -88,6 +90,7 @@ private: int measure() override { return 0; } int collect() override; int probe() override; + int configure(); math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; @@ -104,4 +107,6 @@ private: int write_command(uint16_t command); uint16_t _scale{0}; + const bool _keep_retrying; + bool _require_initialization{true}; }; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp index da8a898015..3f584d1dfd 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp @@ -38,7 +38,8 @@ extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { - SDP3X *instance = new SDP3X(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); + SDP3X *instance = new SDP3X(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address, + cli.custom1 == 1); if (instance == nullptr) { PX4_ERR("alloc failed"); @@ -63,18 +64,28 @@ SDP3X::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21); + PRINT_MODULE_USAGE_PARAM_FLAG('k', "if initialization (probing) fails, keep retrying periodically", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } int sdp3x_airspeed_main(int argc, char *argv[]) { + int ch; using ThisDriver = SDP3X; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 100000; cli.i2c_address = I2C_ADDRESS_1_SDP3X; - const char *verb = cli.parseDefaultArguments(argc, argv); + while ((ch = cli.getopt(argc, argv, "k")) != EOF) { + switch (ch) { + case 'k': // keep retrying + cli.custom1 = 1; + break; + } + } + + const char *verb = cli.optarg(); if (!verb) { ThisDriver::print_usage();