From 3b7b2dc8712110b06788855837ea2f11fcfe9f19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 3 Mar 2020 19:02:11 +0100 Subject: [PATCH] differential_pressure sensors: use driver base class --- ROMFS/px4fmu_common/init.d/rc.sensors | 11 +- ROMFS/px4fmu_test/init.d/rc.sensors | 20 +- boards/av/x-v1/init/rc.board_sensors | 2 +- posix-configs/rpi/px4_fw.config | 2 +- .../ets/ets_airspeed.cpp | 242 ++++------------- .../ms4525/ms4525_airspeed.cpp | 256 +++++------------- .../differential_pressure/ms5525/MS5525.cpp | 5 +- .../differential_pressure/ms5525/MS5525.hpp | 29 +- .../ms5525/MS5525_main.cpp | 205 +++----------- .../differential_pressure/sdp3x/SDP3X.cpp | 2 +- .../differential_pressure/sdp3x/SDP3X.hpp | 28 +- .../sdp3x/SDP3X_main.cpp | 220 +++------------ src/lib/drivers/airspeed/airspeed.cpp | 76 +----- src/lib/drivers/airspeed/airspeed.h | 40 +-- 14 files changed, 248 insertions(+), 890 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 1dbe8cb403..86d43940ca 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,7 +24,8 @@ if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ] then if param compare CBRK_AIRSPD_CHK 0 then - sdp3x_airspeed start -a + sdp3x_airspeed start -X + sdp3x_airspeed start -X -a 0x22 # Pixhawk 2.1 has a MS5611 on I2C which gets wrongly # detected as MS5525 because the chip manufacturer was so @@ -32,14 +33,14 @@ then # register. if [ $BOARD_FMUV3 = 21 ] then - ms5525_airspeed start -b 2 + ms5525_airspeed start -X -b 2 else - ms5525_airspeed start -a + ms5525_airspeed start -X fi - ms4525_airspeed start -a + ms4525_airspeed start -X - ets_airspeed start -a + ets_airspeed start -X fi fi diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index 21188f317c..c9bc1a3bd1 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -21,31 +21,19 @@ if adc start then fi -if sdp3x_airspeed start +if sdp3x_airspeed start -X then fi -if ms5525_airspeed start +if ms5525_airspeed start -X then fi -if ms5525_airspeed start -b 2 +if ms4525_airspeed start -X then fi -if ms4525_airspeed start -then -fi - -if ms4525_airspeed start -b 2 -then -fi - -if ets_airspeed start -then -fi - -if ets_airspeed start -b 1 +if ets_airspeed start -X then fi diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index f3cf068b7b..1675c87db1 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -11,7 +11,7 @@ lps22hb -s start lsm303agr -R 4 start -ms4525_airspeed -T 4515 -b 3 start +ms4525_airspeed -T 4515 -I -b 3 start if ! param greater SENS_EN_PMW3901 0 then diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index b81d10ac98..7ed8c4083e 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -30,7 +30,7 @@ adc start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx -ms4525_airspeed start +ms4525_airspeed start -X rc_update start sensors start commander start diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp index 7e04a36909..9612bb6ed9 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ets_airspeed.cpp @@ -42,6 +42,8 @@ #include #include +#include +#include /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ @@ -59,21 +61,22 @@ /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ -class ETSAirspeed : public Airspeed +class ETSAirspeed : public Airspeed, public I2CSPIDriver { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); + ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS, + const char *path = ETS_PATH); + virtual ~ETSAirspeed() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); protected: - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; int measure() override; int collect() override; - }; /* @@ -81,8 +84,9 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, const char *path) + : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; } @@ -157,7 +161,7 @@ ETSAirspeed::collect() } void -ETSAirspeed::Run() +ETSAirspeed::RunImpl() { int ret; @@ -170,8 +174,9 @@ ETSAirspeed::Run() if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ - start(); + _collect_phase = false; _sensor_ok = false; + ScheduleNow(); return; } @@ -206,206 +211,63 @@ ETSAirspeed::Run() ScheduleDelayed(CONVERSION_INTERVAL); } -/** - * Local functions in support of the shell command. - */ -namespace ets_airspeed +I2CSPIDriverBase *ETSAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + ETSAirspeed *instance = new ETSAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); -ETSAirspeed *g_dev = nullptr; - -int start(); -int start_bus(int i2c_bus); -int stop(); -int reset(); -int info(); - -/** - * Attempt to start driver on all available I2C busses. - * - * This function will return as soon as the first sensor - * is detected on one of the available busses or if no - * sensors are detected. - * - */ -int -start() -{ - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - return PX4_ERROR; + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + instance->ScheduleNow(); + return instance; } -/** - * Start the driver on a specific bus. - * - * This function only returns if the sensor is up and running - * or could not be detected successfully. - */ -int -start_bus(int i2c_bus) + +void +ETSAirspeed::print_usage() { - int fd; - - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - /* create the driver */ - g_dev = new ETSAirspeed(i2c_bus); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(AIRSPEED0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return PX4_OK; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return PX4_ERROR; -} - -/** - * Stop the driver - */ -int -stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - return PX4_OK; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = px4_open(ETS_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - return PX4_OK; -} - -} // namespace - - -static void -ets_airspeed_usage() -{ - PX4_INFO("usage: ets_airspeed command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); - PX4_INFO("\t-a --all"); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|reset|info"); + PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } int ets_airspeed_main(int argc, char *argv[]) { - int i2c_bus = PX4_I2C_BUS_DEFAULT; + using ThisDriver = ETSAirspeed; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - bool start_all = false; + const char *verb = cli.parseDefaultArguments(argc, argv); - while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - - default: - ets_airspeed_usage(); - return 0; - } - } - - if (myoptind >= argc) { - ets_airspeed_usage(); + if (!verb) { + ThisDriver::print_usage(); return -1; } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return ets_airspeed::start(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3); - } else { - return ets_airspeed::start_bus(i2c_bus); - } + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return ets_airspeed::stop(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return ets_airspeed::reset(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - ets_airspeed_usage(); - return 0; + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index b73269b08b..a935b6ad28 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -51,6 +51,8 @@ #include #include +#include +#include #include #include @@ -74,18 +76,22 @@ enum MS_DEVICE_TYPE { #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ -class MEASAirspeed : public Airspeed +class MEASAirspeed : public Airspeed, public I2CSPIDriver { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); + MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_MS4525DO, + const char *path = PATH_MS4525); + + virtual ~MEASAirspeed() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); protected: - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; int measure() override; int collect() override; @@ -105,7 +111,9 @@ protected: */ extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path) +MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, const char *path) + : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; } @@ -221,7 +229,7 @@ MEASAirspeed::collect() } void -MEASAirspeed::Run() +MEASAirspeed::RunImpl() { int ret; @@ -233,8 +241,9 @@ MEASAirspeed::Run() if (OK != ret) { /* restart the measurement state machine */ - start(); + _collect_phase = false; _sensor_ok = false; + ScheduleNow(); return; } @@ -336,216 +345,83 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) #endif // defined(ADC_SCALED_V5_SENSE) } -/** - * Local functions in support of the shell command. - */ -namespace meas_airspeed +I2CSPIDriverBase *MEASAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + MEASAirspeed *instance = new MEASAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, + cli.i2c_address); -MEASAirspeed *g_dev = nullptr; - -int start(); -int start_bus(int i2c_bus, int address); -int stop(); -int reset(); - -/** -* Attempt to start driver on all available I2C busses. -* -* This function will return as soon as the first sensor -* is detected on one of the available busses or if no -* sensors are detected. -* -*/ -int -start() -{ - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i], I2C_ADDRESS_MS4525DO) == PX4_OK) { - return PX4_OK; - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - return PX4_ERROR; + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + instance->ScheduleNow(); + return instance; } -/** - * Start the driver on a specific bus. - * - * This function call only returns once the driver is up and running - * or failed to detect the sensor. - */ -int -start_bus(int i2c_bus, int address) + +void +MEASAirspeed::print_usage() { - int fd; - - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, address, PATH_MS4525); - - /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(PATH_MS4525, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return PX4_OK; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return PX4_ERROR; -} - -/** - * Stop the driver - */ -int -stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - return PX4_OK; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = px4_open(PATH_MS4525, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - return PX4_OK; -} - -} // namespace - - -static void -meas_airspeed_usage() -{ - PX4_INFO("usage: ms4525 command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); - PX4_INFO("\t-a --all"); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|reset"); + PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_STRING('T', "4525", "4525|4515", "Device type", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } int ms4525_airspeed_main(int argc, char *argv[]) { - int i2c_bus = PX4_I2C_BUS_DEFAULT; - - int myoptind = 1; int ch; - const char *myoptarg = nullptr; - + using ThisDriver = MEASAirspeed; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; int device_type = DEVICE_TYPE_MS4525; - bool start_all = false; - - while ((ch = px4_getopt(argc, argv, "ab:T:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { switch (ch) { - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - case 'T': - device_type = atoi(myoptarg); + device_type = atoi(cli.optarg()); break; - - default: - meas_airspeed_usage(); - return 0; } } - if (myoptind >= argc) { - meas_airspeed_usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return meas_airspeed::start(); + if (device_type == DEVICE_TYPE_MS4525) { + cli.i2c_address = I2C_ADDRESS_MS4525DO; - } else if (device_type == DEVICE_TYPE_MS4515) { - return meas_airspeed::start_bus(i2c_bus, I2C_ADDRESS_MS4515DO); - - } else if (device_type == DEVICE_TYPE_MS4525) { - return meas_airspeed::start_bus(i2c_bus, I2C_ADDRESS_MS4525DO); - } + } else { + cli.i2c_address = I2C_ADDRESS_MS4515DO; } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return meas_airspeed::stop(); + BusInstanceIterator iterator(MODULE_NAME, cli, + DRV_DIFF_PRESS_DEVTYPE_MS4525); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return meas_airspeed::reset(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - meas_airspeed_usage(); - return 0; + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp index 41e716b90a..aa5e3f6ea5 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.cpp @@ -269,7 +269,7 @@ MS5525::collect() } void -MS5525::Run() +MS5525::RunImpl() { int ret = PX4_ERROR; @@ -280,8 +280,9 @@ MS5525::Run() if (OK != ret) { /* restart the measurement state machine */ - start(); + _collect_phase = false; _sensor_ok = false; + ScheduleNow(); return; } diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525/MS5525.hpp index dc3fe568ae..46480d6529 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.hpp @@ -31,13 +31,14 @@ * ****************************************************************************/ -#ifndef DRIVERS_MS5525_AIRSPEED_HPP_ -#define DRIVERS_MS5525_AIRSPEED_HPP_ +#pragma once #include #include #include #include +#include +#include /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76; @@ -49,24 +50,26 @@ static constexpr unsigned MEAS_RATE = 100; static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f; static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */ -class MS5525 : public Airspeed +class MS5525 : public Airspeed, public I2CSPIDriver { public: - MS5525(uint8_t bus, uint8_t address = I2C_ADDRESS_1_MS5525DSO, const char *path = PATH_MS5525) : - Airspeed(bus, address, CONVERSION_INTERVAL, path) + MS5525(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_MS5525DSO, + const char *path = PATH_MS5525) : + Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { } - ~MS5525() override = default; + virtual ~MS5525() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); private: - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure() override; int collect() override; @@ -127,5 +130,3 @@ private: uint8_t prom_crc4(uint16_t n_prom[]) const; }; - -#endif /* DRIVERS_MS5525_AIRSPEED_HPP_ */ diff --git a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp b/src/drivers/differential_pressure/ms5525/MS5525_main.cpp index c2ebdc7c1a..9d9085e246 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525_main.cpp @@ -33,199 +33,66 @@ #include "MS5525.hpp" -// Driver 'main' command. extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]); -// Local functions in support of the shell command. -namespace ms5525_airspeed +I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { -MS5525 *g_dev = nullptr; + MS5525 *instance = new MS5525(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); -int start(); -int start_bus(uint8_t i2c_bus); -int stop(); -int reset(); - -/** -* Attempt to start driver on all available I2C busses. -* -* This function will return as soon as the first sensor -* is detected on one of the available busses or if no -* sensors are detected. -* -*/ -int -start() -{ - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - return PX4_ERROR; + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + instance->ScheduleNow(); + return instance; } -/** - * Start the driver on a specific bus. - * - * This function call only returns once the driver is up and running - * or failed to detect the sensor. - */ -int -start_bus(uint8_t i2c_bus) + +void +MS5525::print_usage() { - int fd = -1; - - if (g_dev != nullptr) { - PX4_ERR("already started"); - goto fail; - } - - g_dev = new MS5525(i2c_bus, I2C_ADDRESS_1_MS5525DSO, PATH_MS5525); - - /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) { - goto fail; - } - - /* try to initialize */ - if (g_dev->init() != PX4_OK) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(PATH_MS5525, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return PX4_OK; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return PX4_ERROR; -} - -// stop the driver -int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - return PX4_OK; -} - -// reset the driver -int reset() -{ - int fd = px4_open(PATH_MS5525, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed "); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - return PX4_OK; -} - -} // namespace ms5525_airspeed - - -static void -ms5525_airspeed_usage() -{ - PX4_INFO("usage: ms5525_airspeed command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); - PX4_INFO("\t-a --all"); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|reset"); + PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } int ms5525_airspeed_main(int argc, char *argv[]) { - uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT; + using ThisDriver = MS5525; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - bool start_all = false; + const char *verb = cli.parseDefaultArguments(argc, argv); - while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - - default: - ms5525_airspeed_usage(); - return 0; - } - } - - if (myoptind >= argc) { - ms5525_airspeed_usage(); + if (!verb) { + ThisDriver::print_usage(); return -1; } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return ms5525_airspeed::start(); + BusInstanceIterator iterator(MODULE_NAME, cli, + DRV_DIFF_PRESS_DEVTYPE_MS5525); - } else { - return ms5525_airspeed::start_bus(i2c_bus); - } + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return ms5525_airspeed::stop(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return ms5525_airspeed::reset(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - ms5525_airspeed_usage(); - return 0; + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 4bc4d5b68f..56a8a90b7d 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -166,7 +166,7 @@ SDP3X::collect() } void -SDP3X::Run() +SDP3X::RunImpl() { int ret = PX4_ERROR; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 4d43169c9a..b4dd41a5c7 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -39,13 +39,14 @@ * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_Differential_Pressure_Sensors_SDP3x_Digital_Datasheet_V0.8.pdf */ -#ifndef DRIVERS_SDP3X_AIRSPEED_HPP_ -#define DRIVERS_SDP3X_AIRSPEED_HPP_ +#pragma once #include #include #include #include +#include +#include #define I2C_ADDRESS_1_SDP3X 0x21 #define I2C_ADDRESS_2_SDP3X 0x22 @@ -67,21 +68,26 @@ #define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */ -class SDP3X : public Airspeed +class SDP3X : public Airspeed, public I2CSPIDriver { public: - SDP3X(int bus, int address = I2C_ADDRESS_1_SDP3X, const char *path = PATH_SDP3X) : - Airspeed(bus, address, CONVERSION_INTERVAL, path) + SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X, + const char *path = PATH_SDP3X) : + Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { } + virtual ~SDP3X() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + private: - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; int measure() override { return 0; } int collect() override; int probe() override; @@ -102,5 +108,3 @@ private: uint16_t _scale{0}; }; - -#endif /* DRIVERS_SDP3X_AIRSPEED_HPP_ */ diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp index 6f5f47f09b..34adb0911a 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp @@ -33,213 +33,67 @@ #include "SDP3X.hpp" -// Driver 'main' command. extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); -// Local functions in support of the shell command. -namespace sdp3x_airspeed +I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { -SDP3X *g_dev = nullptr; + SDP3X *instance = new SDP3X(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); -int start(); -int start_bus(uint8_t i2c_bus); -int stop(); -int reset(); - -/** - * Attempt to start driver on all available I2C busses. - * - * This function will return as soon as the first sensor - * is detected on one of the available busses or if no - * sensors are detected. - * - */ -int -start() -{ - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - return PX4_ERROR; + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + instance->ScheduleNow(); + return instance; } -/** - * Start the driver on a specific bus. - * - * This function call only returns once the driver is up and running - * or failed to detect the sensor. - */ -int -start_bus(uint8_t i2c_bus) + +void +SDP3X::print_usage() { - int fd = -1; - - if (g_dev != nullptr) { - PX4_WARN("already started"); - return PX4_ERROR; - } - - g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_1_SDP3X, PATH_SDP3X); - - /* check if the SDP3XDSO was instantiated */ - if (g_dev == nullptr) { - goto fail; - } - - /* try the next SDP3XDSO if init fails */ - if (g_dev->init() != PX4_OK) { - delete g_dev; - - g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X); - - /* check if the SDP3XDSO was instantiated */ - if (g_dev == nullptr) { - PX4_ERR("SDP3X was not instantiated (RAM)"); - goto fail; - } - - /* both versions failed if the init for the SDP3XDSO fails, give up */ - if (g_dev->init() != PX4_OK) { - goto fail; - } - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(PATH_SDP3X, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return PX4_OK; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return PX4_ERROR; -} - -// stop the driver -int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - return PX4_OK; -} - -// reset the driver -int reset() -{ - int fd = px4_open(PATH_SDP3X, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed "); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - return PX4_OK; -} - -} // namespace sdp3x_airspeed - - -static void -sdp3x_airspeed_usage() -{ - PX4_INFO("usage: sdp3x_airspeed command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); - PX4_INFO("\t-a --all"); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|reset"); + PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } int sdp3x_airspeed_main(int argc, char *argv[]) { - uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT; + using ThisDriver = SDP3X; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; + cli.i2c_address = I2C_ADDRESS_1_SDP3X; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - bool start_all = false; + const char *verb = cli.parseDefaultArguments(argc, argv); - while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - - default: - sdp3x_airspeed_usage(); - return 0; - } - } - - if (myoptind >= argc) { - sdp3x_airspeed_usage(); + if (!verb) { + ThisDriver::print_usage(); return -1; } + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31); - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return sdp3x_airspeed::start(); - - } else { - return sdp3x_airspeed::start_bus(i2c_bus); - } + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return sdp3x_airspeed::stop(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return sdp3x_airspeed::reset(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - sdp3x_airspeed_usage(); - return 0; + ThisDriver::print_usage(); + return -1; } diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp index e02c99cf5d..3f52a956e7 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/lib/drivers/airspeed/airspeed.cpp @@ -56,11 +56,10 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : - I2C("Airspeed", path, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval, const char *path) : + I2C("Airspeed", path, bus, address, bus_frequency), _sensor_ok(false), - _measure_interval(0), + _measure_interval(conversion_interval), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_orb_class_instance(-1), @@ -73,9 +72,6 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha Airspeed::~Airspeed() { - /* make sure we are truly inactive */ - stop(); - if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); } @@ -121,57 +117,6 @@ int Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = USEC2TICK(_conversion_interval); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < _conversion_interval) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - break; - case AIRSPEEDIOCSSCALE: { struct airspeed_scale *s = (struct airspeed_scale *)arg; _diff_pres_offset = s->offset_pa; @@ -184,18 +129,3 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -void -Airspeed::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -Airspeed::stop() -{ - ScheduleClear(); -} diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h index c83dce0ac7..f905af3288 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/lib/drivers/airspeed/airspeed.h @@ -42,34 +42,28 @@ #include #include #include -#include -/* Default I2C bus */ -static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION; - -class __EXPORT Airspeed : public device::I2C, public px4::ScheduledWorkItem +class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval, const char *path); + Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval, const char *path); virtual ~Airspeed(); - virtual int init(); + int init() override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; private: - /* this class has pointer data members and should not be copied */ - Airspeed(const Airspeed &); - Airspeed &operator=(const Airspeed &); + Airspeed(const Airspeed &) = delete; + Airspeed &operator=(const Airspeed &) = delete; protected: - virtual int probe(); + int probe() override; /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - virtual void Run() = 0; virtual int measure() = 0; virtual int collect() = 0; @@ -88,26 +82,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * add a new report to the reports queue - * - * @param report differential_pressure_s report - */ - void new_report(const differential_pressure_s &report); };