From dbb53044ce1a5c62dd35e8d0375a55adef013e35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 26 Feb 2020 14:56:30 +0100 Subject: [PATCH] refactor ms5611: use driver base class Also: remove device type auto-detection as it will not work with together with the new SPI board config (which specifies a specific device type) --- boards/intel/aerofc-v1/init/rc.board_sensors | 10 +- boards/px4/fmu-v2/init/rc.board_sensors | 10 +- boards/px4/fmu-v2/src/spi.cpp | 2 + boards/px4/fmu-v3/init/rc.board_sensors | 10 +- boards/px4/fmu-v3/src/spi.cpp | 2 + boards/uvify/core/init/rc.board_extras | 5 +- boards/uvify/core/init/rc.board_sensors | 5 +- boards/uvify/core/src/spi.cpp | 1 + .../common/include/px4_platform_common/spi.h | 2 +- posix-configs/ocpoc/px4.config | 2 +- posix-configs/rpi/px4.config | 2 +- posix-configs/rpi/px4_fw.config | 2 +- posix-configs/rpi/px4_test.config | 2 +- src/drivers/barometer/ms5611/MS5611.hpp | 48 ++-- src/drivers/barometer/ms5611/ms5611.cpp | 70 ++---- src/drivers/barometer/ms5611/ms5611.h | 8 +- src/drivers/barometer/ms5611/ms5611_i2c.cpp | 10 +- src/drivers/barometer/ms5611/ms5611_main.cpp | 224 +++++------------- src/drivers/barometer/ms5611/ms5611_spi.cpp | 23 +- 19 files changed, 142 insertions(+), 296 deletions(-) diff --git a/boards/intel/aerofc-v1/init/rc.board_sensors b/boards/intel/aerofc-v1/init/rc.board_sensors index d41b49e674..10c55843a3 100644 --- a/boards/intel/aerofc-v1/init/rc.board_sensors +++ b/boards/intel/aerofc-v1/init/rc.board_sensors @@ -5,7 +5,10 @@ aerofc_adc start -ms5611 -T 0 start +if ! ms5611 -T 5607 start +then + ms5611 start +fi mpu9250 -s -R 14 start # Possible external compasses @@ -16,4 +19,7 @@ ist8310 -I -R 4 start ll40ls start i2c -a # Internal SPI (auto detect ms5611 or ms5607) -ms5611 -T 0 -s start +if ! ms5611 -T 5607 -s start +then + ms5611 -s start +fi diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 9322e413b6..681ee97897 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -16,11 +16,12 @@ hmc5883 -C -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -s -T 20608 start -# External SPI -ms5611 -S start # Internal SPI (auto detect ms5611 or ms5607) -ms5611 -T 0 -s start +if ! ms5611 -T 5607 -s start +then + ms5611 -s start +fi set BOARD_FMUV3 0 @@ -67,9 +68,6 @@ then # sensor heating is available, but we disable it for now param set SENS_EN_THERMAL 0 - # External SPI - ms5611 -S start - # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start diff --git a/boards/px4/fmu-v2/src/spi.cpp b/boards/px4/fmu-v2/src/spi.cpp index b7cc51059e..74f3c19923 100644 --- a/boards/px4/fmu-v2/src/spi.cpp +++ b/boards/px4/fmu-v2/src/spi.cpp @@ -43,6 +43,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) @@ -59,6 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortC, GPIO::Pin1}), // HMC5983 initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 786c139a2b..a40af652b6 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -17,11 +17,12 @@ hmc5883 -C -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -s -T 20608 start -# External SPI -ms5611 -S start # Internal SPI (auto detect ms5611 or ms5607) -ms5611 -T 0 -s start +if ! ms5611 -T 5607 -s start +then + ms5611 -s start +fi set BOARD_FMUV3 0 @@ -75,9 +76,6 @@ then ak09916 -X -R 6 start fi - # External SPI - ms5611 -S start - # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start diff --git a/boards/px4/fmu-v3/src/spi.cpp b/boards/px4/fmu-v3/src/spi.cpp index b7cc51059e..74f3c19923 100644 --- a/boards/px4/fmu-v3/src/spi.cpp +++ b/boards/px4/fmu-v3/src/spi.cpp @@ -43,6 +43,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) @@ -59,6 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortC, GPIO::Pin1}), // HMC5983 initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) diff --git a/boards/uvify/core/init/rc.board_extras b/boards/uvify/core/init/rc.board_extras index 39248b9002..181dd964c9 100644 --- a/boards/uvify/core/init/rc.board_extras +++ b/boards/uvify/core/init/rc.board_extras @@ -14,5 +14,8 @@ then # It does not start EKF2 in the beginning which is strange behaviour. but 3 seconds hack. # We intentionally put this initialization to here for delayed initialization. sleep 4 - ms5611 -T 0 -X start + if ! ms5611 -T 5607 -X start + then + ms5611 -X start + fi fi diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors index 4b937a6541..7bf61d6272 100644 --- a/boards/uvify/core/init/rc.board_sensors +++ b/boards/uvify/core/init/rc.board_sensors @@ -6,7 +6,10 @@ adc start # Internal SPI -ms5611 -T 0 -s start +if ! ms5611 -T 5607 -s start +then + ms5611 -s start +fi # Draco-R if param compare SYS_AUTOSTART 6002 diff --git a/boards/uvify/core/src/spi.cpp b/boards/uvify/core/src/spi.cpp index 6db9783f74..6c8c0f2cb2 100644 --- a/boards/uvify/core/src/spi.cpp +++ b/boards/uvify/core/src/spi.cpp @@ -45,6 +45,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), }), initSPIBusExternal(SPI::Bus::SPI4, { SPI::CS{GPIO::PortA, GPIO::Pin8}, diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 665f089c59..f4cf023c46 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -48,7 +48,7 @@ typedef uint32_t spi_drdy_gpio_t; -#define SPI_BUS_MAX_DEVICES 5 +#define SPI_BUS_MAX_DEVICES 6 struct px4_spi_bus_device_t { uint32_t cs_gpio; ///< chip-select GPIO (0 if this device is not used) spi_drdy_gpio_t drdy_gpio; ///< data ready GPIO (0 if not set) diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 0e6d4ce00c..e28150fa80 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -17,7 +17,7 @@ param set MAV_SYS_ID 1 mpu9250 -R 10 start hmc5883 start -ms5611 start +ms5611 -s start rgbled start -b 1 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 08864a841e..752c69d050 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -22,7 +22,7 @@ dataman start mpu9250 -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start -ms5611 start +ms5611 -X start navio_rgbled start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 88bb3abf4d..b81d10ac98 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -22,7 +22,7 @@ dataman start mpu9250 -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start -ms5611 start +ms5611 -X start navio_rgbled start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 7665a6a2a9..b07ca9e5e8 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -22,7 +22,7 @@ dataman start mpu9250 -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start -ms5611 start +ms5611 -X start navio_rgbled start diff --git a/src/drivers/barometer/ms5611/MS5611.hpp b/src/drivers/barometer/ms5611/MS5611.hpp index c6d5bb36d5..3003cb8e50 100644 --- a/src/drivers/barometer/ms5611/MS5611.hpp +++ b/src/drivers/barometer/ms5611/MS5611.hpp @@ -36,13 +36,13 @@ #include #include #include +#include #include #include #include "ms5611.h" enum MS56XX_DEVICE_TYPES { - MS56XX_DEVICE = 0, MS5611_DEVICE = 5611, MS5607_DEVICE = 5607, }; @@ -84,20 +84,36 @@ enum MS56XX_DEVICE_TYPES { #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -class MS5611 : public px4::ScheduledWorkItem +class MS5611 : public I2CSPIDriver { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type, + I2CSPIBusOption bus_option, int bus); ~MS5611() override; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + int init(); /** - * Diagnostics - print some basic information about the driver. + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. */ - void print_info(); + void RunImpl(); protected: + void print_status() override; PX4Barometer _px4_barometer; @@ -105,8 +121,6 @@ protected: ms5611::prom_s _prom; - unsigned _measure_interval{0}; - enum MS56XX_DEVICE_TYPES _device_type; bool _collect_phase{false}; unsigned _measure_phase{false}; @@ -127,26 +141,6 @@ protected: */ void start(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void Run() override; - /** * Issue a measurement command for the current state. * diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index e9f7bd01b9..49d5b8bada 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -41,8 +41,9 @@ #include -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type) : - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type, + I2CSPIBusOption bus_option, int bus) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type), _px4_barometer(interface->get_device_id()), _interface(interface), _prom(prom_buf.s), @@ -55,9 +56,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_ MS5611::~MS5611() { - /* make sure we are truly inactive */ - stop(); - // free perf counters perf_free(_sample_perf); perf_free(_measure_perf); @@ -70,17 +68,10 @@ int MS5611::init() { int ret; - bool autodetect = false; /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; - if (_device_type == MS56XX_DEVICE) { - autodetect = true; - /* try first with MS5611 and fallback to MS5607 */ - _device_type = MS5611_DEVICE; - } - while (true) { /* do temperature first */ if (OK != measure()) { @@ -111,22 +102,11 @@ MS5611::init() /* state machine will have generated a report, copy it out */ const sensor_baro_s &brp = _px4_barometer.get(); - if (autodetect) { - if (_device_type == MS5611_DEVICE) { - if (brp.pressure < 520.0f) { - /* This is likely not this device, try again */ - _device_type = MS5607_DEVICE; - _measure_phase = 0; - - continue; - } - - } else if (_device_type == MS5607_DEVICE) { - if (brp.pressure < 520.0f) { - /* Both devices returned a very low pressure; - * have fun on Everest using MS5611 */ - _device_type = MS5611_DEVICE; - } + if (_device_type == MS5607_DEVICE) { + if (brp.pressure < 520.0f) { + /* This is likely not this device, abort */ + ret = -EINVAL; + break; } } @@ -150,7 +130,9 @@ MS5611::init() break; } - start(); + if (ret == 0) { + start(); + } return ret; } @@ -163,17 +145,11 @@ MS5611::start() _measure_phase = 0; /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -MS5611::stop() -{ - ScheduleClear(); + ScheduleDelayed(MS5611_CONVERSION_INTERVAL); } void -MS5611::Run() +MS5611::RunImpl() { int ret; unsigned dummy; @@ -207,20 +183,6 @@ MS5611::Run() /* next phase is measurement */ _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_interval > MS5611_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - MS5611_CONVERSION_INTERVAL); - - return; - } } /* measurement phase */ @@ -366,8 +328,9 @@ MS5611::collect() return OK; } -void MS5611::print_info() +void MS5611::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -376,9 +339,6 @@ void MS5611::print_info() _px4_barometer.print_status(); } -/** - * Local functions in support of the shell command. - */ namespace ms5611 { diff --git a/src/drivers/barometer/ms5611/ms5611.h b/src/drivers/barometer/ms5611/ms5611.h index 6126d8a56f..e4d6ff5156 100644 --- a/src/drivers/barometer/ms5611/ms5611.h +++ b/src/drivers/barometer/ms5611/ms5611.h @@ -90,7 +90,9 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum); -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum); +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency, + spi_mode_e spi_mode); +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, + int bus_frequency); -typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum); diff --git a/src/drivers/barometer/ms5611/ms5611_i2c.cpp b/src/drivers/barometer/ms5611/ms5611_i2c.cpp index 7e67d121e7..63b04dd5cf 100644 --- a/src/drivers/barometer/ms5611/ms5611_i2c.cpp +++ b/src/drivers/barometer/ms5611/ms5611_i2c.cpp @@ -47,7 +47,7 @@ class MS5611_I2C : public device::I2C { public: - MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf); + MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf, int bus_frequency); ~MS5611_I2C() override = default; int read(unsigned offset, void *data, unsigned count) override; @@ -85,13 +85,13 @@ private: }; device::Device * -MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) +MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency) { - return new MS5611_I2C(busnum, prom_buf); + return new MS5611_I2C(busnum, prom_buf, bus_frequency); } -MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", nullptr, bus, 0, 400000), +MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom, int bus_frequency) : + I2C("MS5611_I2C", nullptr, bus, 0, bus_frequency), _prom(prom) { } diff --git a/src/drivers/barometer/ms5611/ms5611_main.cpp b/src/drivers/barometer/ms5611/ms5611_main.cpp index 2e04bd6818..5d098251c2 100644 --- a/src/drivers/barometer/ms5611/ms5611_main.cpp +++ b/src/drivers/barometer/ms5611/ms5611_main.cpp @@ -33,220 +33,110 @@ #include #include +#include +#include + #include "MS5611.hpp" #include "ms5611.h" -enum class MS5611_BUS { - ALL = 0, - I2C_INTERNAL, - I2C_EXTERNAL, - SPI_INTERNAL, - SPI_EXTERNAL -}; - -namespace ms5611 +I2CSPIDriverBase *MS5611::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + ms5611::prom_u prom_buf; + device::Device *interface = nullptr; -// list of supported bus configurations -struct ms5611_bus_option { - MS5611_BUS busid; - MS5611_constructor interface_constructor; - uint8_t busnum; - MS5611 *dev; -} bus_options[] = { -#if defined(PX4_I2C_BUS_ONBOARD) - { MS5611_BUS::I2C_INTERNAL, &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, nullptr }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION) - { MS5611_BUS::I2C_EXTERNAL, &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, nullptr }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION1) - { MS5611_BUS::I2C_EXTERNAL, &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION1, nullptr }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION2) - { MS5611_BUS::I2C_EXTERNAL, &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION2, nullptr }, -#endif -#if defined(PX4_SPI_BUS_BARO) && defined(PX4_SPIDEV_BARO) - { MS5611_BUS::SPI_INTERNAL, &MS5611_spi_interface, PX4_SPI_BUS_BARO, nullptr }, -#endif -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BARO) - { MS5611_BUS::SPI_EXTERNAL, &MS5611_spi_interface, PX4_SPI_BUS_EXT, nullptr }, -#endif -}; - -// find a bus structure for a busid -static struct ms5611_bus_option *find_bus(MS5611_BUS busid) -{ - for (ms5611_bus_option &bus_option : bus_options) { - if ((busid == MS5611_BUS::ALL || - busid == bus_option.busid) && bus_option.dev != nullptr) { + if (iterator.busType() == BOARD_I2C_BUS) { + interface = MS5611_i2c_interface(prom_buf, iterator.devid(), iterator.bus(), cli.bus_frequency); - return &bus_option; - } + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = MS5611_spi_interface(prom_buf, iterator.devid(), iterator.bus(), cli.bus_frequency, cli.spi_mode); } - return nullptr; -} - -static bool start_bus(ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) -{ - prom_u prom_buf; - device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } - if ((interface == nullptr) || (interface->init() != PX4_OK)) { - PX4_WARN("no device on bus %u", (unsigned)bus.busid); + if (interface->init() != OK) { delete interface; - return false; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; } - MS5611 *dev = new MS5611(interface, prom_buf, device_type); + MS5611 *dev = new MS5611(interface, prom_buf, (MS56XX_DEVICE_TYPES)cli.type, iterator.configuredBusOption(), + iterator.bus()); if (dev == nullptr) { - PX4_ERR("alloc failed"); - return false; + delete interface; + return nullptr; } - if (dev->init() != PX4_OK) { - PX4_ERR("driver start failed"); + if (OK != dev->init()) { delete dev; - return false; - } - - bus.dev = dev; - - return true; -} - -static int start(MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) -{ - for (ms5611_bus_option &bus_option : bus_options) { - if (bus_option.dev != nullptr) { - // this device is already started - PX4_WARN("already started"); - continue; - } - - if (busid != MS5611_BUS::ALL && bus_option.busid != busid) { - // not the one that is asked for - continue; - } - - if (start_bus(bus_option, device_type)) { - return PX4_OK; - } - } - - return PX4_ERROR; -} - -static int stop(MS5611_BUS busid) -{ - ms5611_bus_option *bus = find_bus(busid); - - if (bus != nullptr && bus->dev != nullptr) { - delete bus->dev; - bus->dev = nullptr; - - } else { - PX4_WARN("driver not running"); - return PX4_ERROR; + return nullptr; } - return PX4_OK; + return dev; } -static int status(MS5611_BUS busid) +void MS5611::print_usage() { - ms5611_bus_option *bus = find_bus(busid); - - if (bus != nullptr && bus->dev != nullptr) { - bus->dev->print_info(); - return PX4_OK; - } - - PX4_WARN("driver not running"); - return PX4_ERROR; + PRINT_MODULE_USAGE_NAME("ms5611", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611", "Device type", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -static int usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -X (i2c external bus)"); - PX4_INFO(" -I (i2c internal bus)"); - PX4_INFO(" -s (spi internal bus)"); - PX4_INFO(" -S (spi external bus)"); - PX4_INFO(" -T 5611|5607 (default 5611)"); - PX4_INFO(" -T 0 (autodetect version)"); - - return 0; -} - -} // namespace - extern "C" int ms5611_main(int argc, char *argv[]) { - int myoptind = 1; + using ThisDriver = MS5611; int ch; - const char *myoptarg = nullptr; - - MS5611_BUS busid = MS5611_BUS::ALL; - enum MS56XX_DEVICE_TYPES device_type = MS5611_DEVICE; + BusCLIArguments cli{true, true}; + cli.type = MS5611_DEVICE; + uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5611; - while ((ch = px4_getopt(argc, argv, "XISsT:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { switch (ch) { - case 'X': - busid = MS5611_BUS::I2C_EXTERNAL; - break; - - case 'I': - busid = MS5611_BUS::I2C_INTERNAL; - break; - - case 'S': - busid = MS5611_BUS::SPI_EXTERNAL; - break; - - case 's': - busid = MS5611_BUS::SPI_INTERNAL; - break; - case 'T': { - int val = atoi(myoptarg); + int val = atoi(cli.optarg()); if (val == 5611) { - device_type = MS5611_DEVICE; + cli.type = MS5611_DEVICE; + dev_type_driver = DRV_BARO_DEVTYPE_MS5611; } else if (val == 5607) { - device_type = MS5607_DEVICE; - - } else if (val == 0) { - device_type = MS56XX_DEVICE; + cli.type = MS5607_DEVICE; + dev_type_driver = DRV_BARO_DEVTYPE_MS5607; } } break; - - default: - return ms5611::usage(); } } - if (myoptind >= argc) { - return ms5611::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver); if (!strcmp(verb, "start")) { - return ms5611::start(busid, device_type); + return ThisDriver::module_start(cli, iterator); + } - } else if (!strcmp(verb, "stop")) { - return ms5611::stop(busid); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } - } else if (!strcmp(verb, "status")) { - return ms5611::status(busid); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - return ms5611::usage(); + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/barometer/ms5611/ms5611_spi.cpp b/src/drivers/barometer/ms5611/ms5611_spi.cpp index 6447164d4e..544a388638 100644 --- a/src/drivers/barometer/ms5611/ms5611_spi.cpp +++ b/src/drivers/barometer/ms5611/ms5611_spi.cpp @@ -45,12 +45,11 @@ #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) class MS5611_SPI : public device::SPI { public: - MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf); + MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf, int bus_frequency, spi_mode_e spi_mode); virtual ~MS5611_SPI() = default; virtual int init(); @@ -98,24 +97,13 @@ private: }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) +MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency, spi_mode_e spi_mode) { -#ifdef PX4_SPI_BUS_EXT - - if (busnum == PX4_SPI_BUS_EXT) { -#ifdef PX4_SPIDEV_EXT_BARO - return new MS5611_SPI(busnum, PX4_SPIDEV_EXT_BARO, prom_buf); -#else - return nullptr; -#endif - } - -#endif - return new MS5611_SPI(busnum, PX4_SPIDEV_BARO, prom_buf); + return new MS5611_SPI(busnum, devid, prom_buf, bus_frequency, spi_mode); } -MS5611_SPI::MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 20 * 1000 * 1000 /* will be rounded to 10.4 MHz */), +MS5611_SPI::MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf, int bus_frequency, spi_mode_e spi_mode) : + SPI("MS5611_SPI", nullptr, bus, device, spi_mode, bus_frequency), _prom(prom_buf) { } @@ -272,4 +260,3 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) return transfer(send, recv, len); } -#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */