From 16bd1088fa6814dfc6ac4e3ec18747de05516b4c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 29 Dec 2019 13:18:37 -0500 Subject: [PATCH] mpl3115a2: move to PX4Barometer and cleanup --- .../barometer/mpl3115a2/CMakeLists.txt | 12 +- src/drivers/barometer/mpl3115a2/MPL3115A2.cpp | 298 ++++++ .../mpl3115a2/{mpl3115a2.h => MPL3115A2.hpp} | 90 +- src/drivers/barometer/mpl3115a2/mpl3115a2.cpp | 889 ------------------ .../barometer/mpl3115a2/mpl3115a2_i2c.cpp | 209 ---- .../barometer/mpl3115a2/mpl3115a2_main.cpp | 198 ++++ 6 files changed, 535 insertions(+), 1161 deletions(-) create mode 100644 src/drivers/barometer/mpl3115a2/MPL3115A2.cpp rename src/drivers/barometer/mpl3115a2/{mpl3115a2.h => MPL3115A2.hpp} (50%) delete mode 100644 src/drivers/barometer/mpl3115a2/mpl3115a2.cpp delete mode 100644 src/drivers/barometer/mpl3115a2/mpl3115a2_i2c.cpp create mode 100644 src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp diff --git a/src/drivers/barometer/mpl3115a2/CMakeLists.txt b/src/drivers/barometer/mpl3115a2/CMakeLists.txt index 74d452b9ae..c96c24dfd6 100644 --- a/src/drivers/barometer/mpl3115a2/CMakeLists.txt +++ b/src/drivers/barometer/mpl3115a2/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,11 +31,15 @@ # ############################################################################ px4_add_module( - MODULE drivers__mpl3115a2 + MODULE drivers__barometer__mpl3115a2 MAIN mpl3115a2 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS - mpl3115a2_i2c.cpp - mpl3115a2.cpp + MPL3115A2.hpp + MPL3115A2.cpp + mpl3115a2_main.cpp + DEPENDS + drivers_barometer + px4_work_queue ) diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp new file mode 100644 index 0000000000..6e29872e08 --- /dev/null +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MPL3115A2.hpp" + +#define MPL3115A2_ADDRESS 0x60 + +#define MPL3115A2_REG_WHO_AM_I 0x0c +#define MPL3115A2_WHO_AM_I 0xC4 + +#define OUT_P_MSB 0x01 + +#define MPL3115A2_CTRL_REG1 0x26 +# define CTRL_REG1_ALT (1 << 7) +# define CTRL_REG1_RAW (1 << 6) +# define CTRL_REG1_OS_SHIFTS (3) +# define CTRL_REG1_OS_MASK (0x7 << CTRL_REG1_OS_SHIFTS) +# define CTRL_REG1_OS(n) (((n)& 0x7) << CTRL_REG1_OS_SHIFTS) +# define CTRL_REG1_RST (1 << 2) +# define CTRL_REG1_OST (1 << 1) +# define CTRL_REG1_SBYB (1 << 0) + +#define MPL3115A2_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MPL3115A2_OSR 2 /* Over Sample rate of 4 18MS Minimum time between data samples */ +#define MPL3115A2_CTRL_TRIGGER (CTRL_REG1_OST | CTRL_REG1_OS(MPL3115A2_OSR)) + +MPL3115A2::MPL3115A2(const int bus) : + I2C("MPL3115A2", nullptr, bus, MPL3115A2_ADDRESS, 400000), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_barometer(get_device_id()), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) +{ + set_device_type(DRV_BARO_DEVTYPE_MPL3115A2); + _px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MPL3115A2); +} + +MPL3115A2::~MPL3115A2() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); +} + +int MPL3115A2::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + PX4_ERR("init failed"); + return PX4_ERROR; + } + + start(); + + return PX4_OK; +} + +int MPL3115A2::probe() +{ + _retries = 10; + uint8_t whoami = 0; + + if ((RegisterRead(MPL3115A2_REG_WHO_AM_I, &whoami) > 0) && (whoami == MPL3115A2_WHO_AM_I)) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return PX4_OK; + } + + return -EIO; +} + +int MPL3115A2::RegisterRead(uint8_t reg, void *data, unsigned count) +{ + uint8_t cmd = reg; + int ret = transfer(&cmd, 1, (uint8_t *)data, count); + return ret == PX4_OK ? count : ret; +} + +int MPL3115A2::RegisterWrite(uint8_t reg, uint8_t data) +{ + uint8_t buf[2] = { reg, data}; + int ret = transfer(buf, sizeof(buf), NULL, 0); + return ret == PX4_OK ? 2 : ret; +} + +void MPL3115A2::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + ScheduleNow(); +} + +void MPL3115A2::stop() +{ + ScheduleClear(); +} + +int MPL3115A2::reset() +{ + int max = 10; + RegisterWrite(MPL3115A2_CTRL_REG1, CTRL_REG1_RST); + int rv = CTRL_REG1_RST; + int ret = 1; + + while (ret == 1 && (rv & CTRL_REG1_RST) && max--) { + usleep(400); + ret = RegisterRead(MPL3115A2_CTRL_REG1, &rv); + } + + return ret == 1 ? PX4_OK : ret; +} + +void MPL3115A2::Run() +{ + int ret = PX4_ERROR; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (ret == -EIO) { + /* issue a reset command to the sensor */ + reset(); + + /* reset the collection state machine and try again - we need + * to wait 2.8 ms after issuing the sensor reset command + * according to the MPL3115A2 datasheet + */ + _collect_phase = false; + ScheduleDelayed(2800); + return; + } + + if (ret == -EAGAIN) { + /* Ready read it on next cycle */ + ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); + + return; + } + + /* next phase is measurement */ + _collect_phase = false; + } + + /* Look for a ready condition */ + ret = measure(); + + if (ret == -EIO) { + /* issue a reset command to the sensor */ + reset(); + + /* reset the collection state machine and try again */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); +} + +int MPL3115A2::measure() +{ + perf_begin(_measure_perf); + + // Send the command to read the ADC for P and T. + unsigned addr = (MPL3115A2_CTRL_REG1 << 8) | MPL3115A2_CTRL_TRIGGER; + + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + int ret = RegisterWrite((addr >> 8) & 0xff, addr & 0xff); + + if (ret == -EIO) { + perf_count(_comms_errors); + } + + perf_end(_measure_perf); + + return PX4_OK; +} + +int MPL3115A2::collect() +{ + perf_begin(_sample_perf); + + uint8_t ctrl{}; + int ret = RegisterRead(MPL3115A2_CTRL_REG1, (void *)&ctrl, 1); + + if (ret == -EIO) { + perf_end(_sample_perf); + return ret; + } + + if (ctrl & CTRL_REG1_OST) { + perf_end(_sample_perf); + return -EAGAIN; + } + + + + /* read the most recent measurement + * 3 Pressure and 2 temprtture + */ + uint8_t b[3 + 2] {}; + uint8_t reg = OUT_P_MSB; + const hrt_abstime timestamp_sample = hrt_absolute_time(); + ret = transfer(®, 1, &b[0], sizeof(b)); + + if (ret == -EIO) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + +#pragma pack(push, 1) + struct MPL3115A2_data_t { + union { + uint32_t q; + uint16_t w[sizeof(q) / sizeof(uint16_t)]; + uint8_t b[sizeof(q) / sizeof(uint8_t)]; + } pressure; + + union { + uint16_t w; + uint8_t b[sizeof(w)]; + } temperature; + } reading; +#pragma pack(pop) + + reading.pressure.q = ((uint32_t)b[0]) << 18 | ((uint32_t) b[1]) << 10 | (((uint32_t)b[2]) & 0xc0) << 2 | (( + b[2] & 0x30) >> 4); + reading.temperature.w = ((uint16_t) b[3]) << 8 | (b[4] >> 4); + + float T = (float) reading.temperature.b[1] + ((float)(reading.temperature.b[0]) / 16.0f); + float P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f); + + _px4_barometer.set_error_count(perf_event_count(_comms_errors)); + _px4_barometer.set_temperature(T); + _px4_barometer.update(timestamp_sample, P / 100.0f); + + perf_end(_sample_perf); + + return PX4_OK; +} + +void MPL3115A2::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + _px4_barometer.print_status(); +} diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.h b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp similarity index 50% rename from src/drivers/barometer/mpl3115a2/mpl3115a2.h rename to src/drivers/barometer/mpl3115a2/MPL3115A2.hpp index bbebe68e3b..47249f1dfe 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.h +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,80 +32,52 @@ ****************************************************************************/ /** - * @file mpl3115a2.h + * @file MPL3115A2.hpp * - * Shared defines for the mpl3115a2 driver. + * Driver for the MPL3115A2 barometric pressure sensor connected via I2C. */ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include #include -#include -#include -#include -#include +#include +#include #include #include -#include #include #include -#include -#include "board_config.h" +class MPL3115A2 : public device::I2C, public px4::ScheduledWorkItem +{ +public: + MPL3115A2(const int bus); + ~MPL3115A2() override; -#define MPL3115A2_REG_WHO_AM_I 0x0c -#define MPL3115A2_WHO_AM_I 0xC4 + int init() override; + int probe() override; -#define OUT_P_MSB 0x01 + void print_info(); -#define MPL3115A2_CTRL_REG1 0x26 -# define CTRL_REG1_ALT (1 << 7) -# define CTRL_REG1_RAW (1 << 6) -# define CTRL_REG1_OS_SHIFTS (3) -# define CTRL_REG1_OS_MASK (0x7 << CTRL_REG1_OS_SHIFTS) -# define CTRL_REG1_OS(n) (((n)& 0x7) << CTRL_REG1_OS_SHIFTS) -# define CTRL_REG1_RST (1 << 2) -# define CTRL_REG1_OST (1 << 1) -# define CTRL_REG1_SBYB (1 << 0) +private: -/* interface ioctls */ -#define IOCTL_RESET 1 -#define IOCTL_MEASURE 2 + void start(); + void stop(); + int reset(); -typedef begin_packed_struct struct MPL3115A2_data_t { - union { - uint32_t q; - uint16_t w[sizeof(q) / sizeof(uint16_t)]; - uint8_t b[sizeof(q) / sizeof(uint8_t)]; - } pressure; + void Run() override; - union { - uint16_t w; - uint8_t b[sizeof(w)]; - } temperature; -} end_packed_struct MPL3115A2_data_t; + int measure(); + int collect(); -/* interface factories */ -extern device::Device *MPL3115A2_i2c_interface(uint8_t busnum); -extern device::Device *MPL3115A2_sim_interface(uint8_t busnum); -typedef device::Device *(*MPL3115A2_constructor)(uint8_t busnum); + int RegisterRead(uint8_t reg, void *data, unsigned count = 1); + int RegisterWrite(uint8_t reg, uint8_t data); + + PX4Barometer _px4_barometer; + + bool _collect_phase{false}; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; +}; diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp deleted file mode 100644 index 6bad1cac6c..0000000000 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp +++ /dev/null @@ -1,889 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017-2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mpl3115a2.cpp - * - * Driver for the MPL3115A2 barometric pressure sensor connected via I2C. - */ - -#include "mpl3115a2.h" - -enum MPL3115A2_BUS { - MPL3115A2_BUS_ALL = 0, - MPL3115A2_BUS_I2C_INTERNAL, - MPL3115A2_BUS_I2C_EXTERNAL, -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MPL3115A2 internal constants and data structures. - */ - -#define MPL3115A2_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MPL3115A2_OSR 2 /* Over Sample rate of 4 18MS Minimum time between data samples */ -#define MPL3115A2_CTRL_TRIGGER (CTRL_REG1_OST | CTRL_REG1_OS(MPL3115A2_OSR)) -#define MPL3115A2_BARO_DEVICE_PATH_EXT "/dev/mpl3115a2_ext" -#define MPL3115A2_BARO_DEVICE_PATH_INT "/dev/mpl3115a2_int" - -class MPL3115A2 : public cdev::CDev, public px4::ScheduledWorkItem -{ -public: - MPL3115A2(device::Device *interface, const char *path); - ~MPL3115A2(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - device::Device *_interface; - - unsigned _measure_interval; - - ringbuffer::RingBuffer *_reports; - bool _collect_phase; - - /* */ - float _P; - float _T; - - orb_advert_t _baro_topic; - int _orb_class_instance; - int _class_instance; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - - /** - * Initialize the automatic measurement state machine and start it. - * - * @param delay_us the number of microseconds before executing the next cycle - * - * @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(unsigned delay_us = 1); - - /** - * 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. - * - * @return OK if the measurement command was successful. - */ - virtual int measure(); - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); - -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int mpl3115a2_main(int argc, char *argv[]); - -MPL3115A2::MPL3115A2(device::Device *interface, const char *path) : - CDev(path), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), - _interface(interface), - _measure_interval(0), - _reports(nullptr), - _collect_phase(false), - _P(0), - _T(0), - _baro_topic(nullptr), - _orb_class_instance(-1), - _class_instance(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "mpl3115a2_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "mpl3115a2_measure")), - _comms_errors(perf_alloc(PC_COUNT, "mpl3115a2_com_err")) -{ - _interface->set_device_type(DRV_BARO_DEVTYPE_MPL3115A2); -} - -MPL3115A2::~MPL3115A2() -{ - /* make sure we are truly inactive */ - stop(); - - if (_class_instance != -1) { - unregister_class_devname(get_devname(), _class_instance); - } - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_measure_perf); - perf_free(_comms_errors); - - delete _interface; -} - -int -MPL3115A2::init() -{ - int ret; - - ret = CDev::init(); - - if (ret != OK) { - PX4_DEBUG("CDev init failed"); - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); - - if (_reports == nullptr) { - PX4_DEBUG("can't get memory for reports"); - ret = -ENOMEM; - goto out; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - sensor_baro_s brp; - _reports->flush(); - - while (true) { - - /* First reading */ - do { - ret = measure(); - - if (ret == -EAGAIN) { - usleep(500); - } - } while (ret == -EAGAIN); - - if (ret != OK) { - ret = -EIO; - break; - } - - /* First reading */ - do { - ret = collect(); - - if (ret == -EAGAIN) { - usleep(500); - } - } while (ret == -EAGAIN); - - if (ret != OK) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - _reports->get(&brp); - - // DEVICE_LOG("altitude (%u) = %.2f", _device_type, (double)brp.altitude); - - /* ensure correct devid */ - brp.device_id = _interface->get_device_id(); - - ret = OK; - - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (_interface->external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - warnx("failed to create sensor_baro publication"); - } - - break; - } - -out: - return ret; -} - -ssize_t -MPL3115A2::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_baro_s); - sensor_baro_s *brp = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(brp)) { - ret += sizeof(*brp); - brp++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* First reading */ - do { - ret = measure(); - - if (ret == -EAGAIN) { - usleep(1000); - } - } while (ret == -EAGAIN); - - if (ret != OK) { - ret = -EIO; - break; - } - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) { - ret = sizeof(*brp); - } - - } while (0); - - return ret; -} - -int -MPL3115A2::ioctl(struct file *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 = MPL3115A2_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 < MPL3115A2_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; - } - } - } - - case SENSORIOCRESET: { - /* - * Since we are initialized, we do not need to do anything, since the - * PROM is correctly read and the part does not need to be configured. - */ - unsigned int dummy; - _interface->ioctl(IOCTL_RESET, dummy); - return OK; - } - - default: - break; - } - - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return CDev::ioctl(filp, cmd, arg); -} - -void -MPL3115A2::start(unsigned delay_us) -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleDelayed(delay_us); -} - -void -MPL3115A2::stop() -{ - ScheduleClear(); -} - -void -MPL3115A2::Run() -{ - int ret; - unsigned dummy; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (ret == -EIO) { - /* issue a reset command to the sensor */ - _interface->ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again - we need - * to wait 2.8 ms after issuing the sensor reset command - * according to the MPL3115A2 datasheet - */ - start(2800); - return; - } - - if (ret == -EAGAIN) { - - /* Ready read it on next cycle */ - ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); - - return; - } - - /* next phase is measurement */ - _collect_phase = false; - } - - /* Look for a ready condition */ - - ret = measure(); - - if (ret == -EIO) { - /* issue a reset command to the sensor */ - _interface->ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL); -} - -int -MPL3115A2::measure() -{ - int ret; - - perf_begin(_measure_perf); - - /* - * Send the command to read the ADC for P and T. - */ - unsigned addr = (MPL3115A2_CTRL_REG1 << 8) | MPL3115A2_CTRL_TRIGGER; - ret = _interface->ioctl(IOCTL_MEASURE, addr); - - if (ret == -EIO) { - perf_count(_comms_errors); - } - - perf_end(_measure_perf); - - return PX4_OK; -} - -int -MPL3115A2::collect() -{ - int ret; - MPL3115A2_data_t reading; - uint8_t ctrl; - - perf_begin(_sample_perf); - - ret = _interface->read(MPL3115A2_CTRL_REG1, (void *)&ctrl, 1); - - if (ret == -EIO) { - perf_end(_sample_perf); - return ret; - } - - if (ctrl & CTRL_REG1_OST) { - perf_end(_sample_perf); - return -EAGAIN; - } - - sensor_baro_s report; - - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - - report.error_count = perf_event_count(_comms_errors); - - /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->read(OUT_P_MSB, (void *)&reading, sizeof(reading)); - - if (ret == -EIO) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - _T = (float) reading.temperature.b[1] + ((float)(reading.temperature.b[0]) / 16.0f); - _P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f); - - report.temperature = _T; - report.pressure = _P / 100.0f; /* convert to millibar */ - - /* return device ID */ - report.device_id = _interface->get_device_id(); - - /* publish it */ - if (_baro_topic != nullptr) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - } - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - perf_end(_sample_perf); - - return OK; -} - -void -MPL3115A2::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); - - sensor_baro_s brp = {}; - _reports->get(&brp); - print_message(brp); -} - -/** - * Local functions in support of the shell command. - */ -namespace mpl3115a2 -{ - -/* - list of supported bus configurations - */ -struct mpl3115a2_bus_option { - enum MPL3115A2_BUS busid; - const char *devpath; - MPL3115A2_constructor interface_constructor; - uint8_t busnum; - MPL3115A2 *dev; -} bus_options[] = { -#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) - { MPL3115A2_BUS_SPI_EXTERNAL, "/dev/mpl3115a2_spi_ext", &MPL3115A2_spi_interface, PX4_SPI_BUS_EXT, NULL }, -#endif -#ifdef PX4_SPIDEV_BARO - { MPL3115A2_BUS_SPI_INTERNAL, "/dev/mpl3115a2_spi_int", &MPL3115A2_spi_interface, PX4_SPI_BUS_BARO, NULL }, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { MPL3115A2_BUS_I2C_INTERNAL, "/dev/mpl3115a2_int", &MPL3115A2_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION - { MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION1 - { MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext1", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION2 - { MPL3115A2_BUS_I2C_EXTERNAL, "/dev/mpl3115a2_ext2", &MPL3115A2_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct mpl3115a2_bus_option &bus); -struct mpl3115a2_bus_option &find_bus(enum MPL3115A2_BUS busid); -void start(enum MPL3115A2_BUS busid); -void test(enum MPL3115A2_BUS busid); -void reset(enum MPL3115A2_BUS busid); -void info(); -void usage(); - -/** - * Start the driver. - */ -bool -start_bus(struct mpl3115a2_bus_option &bus) -{ - if (bus.dev != nullptr) { - errx(1, "bus option already started"); - } - - device::Device *interface = bus.interface_constructor(bus.busnum); - - if (interface->init() != OK) { - delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new MPL3115A2(interface, bus.devpath); - - if (bus.dev != nullptr && OK != bus.dev->init()) { - delete bus.dev; - bus.dev = NULL; - return false; - } - - int fd = open(bus.devpath, O_RDONLY); - - /* set the poll rate to default, starts automatic data collection */ - if (fd == -1) { - errx(1, "can't open baro device"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - close(fd); - errx(1, "failed setting default poll rate"); - } - - close(fd); - return true; -} - - -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -void -start(enum MPL3115A2_BUS busid) -{ - uint8_t i; - bool started = false; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == MPL3115A2_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (busid != MPL3115A2_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started = started | start_bus(bus_options[i]); - } - - if (!started) { - exit(1); - } - - // one or more drivers started OK - exit(0); -} - - -/** - * find a bus structure for a busid - */ -struct mpl3115a2_bus_option &find_bus(enum MPL3115A2_BUS busid) -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == MPL3115A2_BUS_ALL || - busid == bus_options[i].busid) && bus_options[i].dev != NULL) { - return bus_options[i]; - } - } - - errx(1, "bus %u not started", (unsigned)busid); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test(enum MPL3115A2_BUS busid) -{ - struct mpl3115a2_bus_option &bus = find_bus(busid); - sensor_baro_s report; - ssize_t sz; - int ret; - - int fd; - - fd = open(bus.devpath, O_RDONLY); - - if (fd < 0) { - err(1, "open failed (try 'mpl3115a2 start' if the driver is not running)"); - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - err(1, "immediate read failed"); - } - - print_message(report); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - errx(1, "timed out waiting for sensor data"); - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - err(1, "periodic read failed"); - } - - print_message(report); - } - - close(fd); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset(enum MPL3115A2_BUS busid) -{ - struct mpl3115a2_bus_option &bus = find_bus(busid); - int fd; - - fd = open(bus.devpath, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct mpl3115a2_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - warnx("%s", bus.devpath); - bus.dev->print_info(); - } - } - - exit(0); -} - -void -usage() -{ - warnx("missing command: try 'start', 'info', 'test', 'reset'"); - warnx("options:"); - warnx(" -X (external I2C bus)"); - warnx(" -I (intternal I2C bus)"); - -} - -} // namespace - -int -mpl3115a2_main(int argc, char *argv[]) -{ - enum MPL3115A2_BUS busid = MPL3115A2_BUS_ALL; - int myoptind = 1; - int ch; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "XI", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - busid = MPL3115A2_BUS_I2C_EXTERNAL; - break; - - case 'I': - busid = MPL3115A2_BUS_I2C_INTERNAL; - break; - - default: - mpl3115a2::usage(); - exit(0); - } - } - - if (myoptind >= argc) { - mpl3115a2::usage(); - exit(0); - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - mpl3115a2::start(busid); - } - - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - mpl3115a2::test(busid); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - mpl3115a2::reset(busid); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - mpl3115a2::info(); - } - - mpl3115a2::usage(); - exit(1); -} diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2_i2c.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2_i2c.cpp deleted file mode 100644 index 9e3b9a46ae..0000000000 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2_i2c.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mpl3115a2_i2c.cpp - * - * I2C interface for MPL3115A2 - */ - -#include "mpl3115a2.h" - -#define MPL3115A2_ADDRESS 0x60 - -device::Device *MPL3115A2_i2c_interface(uint8_t busnum); - -class MPL3115A2_I2C : public device::I2C -{ -public: - MPL3115A2_I2C(uint8_t bus); - virtual ~MPL3115A2_I2C() = default; - - virtual int init(); - virtual int read(unsigned offset, void *data, unsigned count); - virtual int ioctl(unsigned operation, unsigned &arg); - -protected: - virtual int probe(); - -private: - - /** - * Send a measure command to the MPL3115A2. - * - * @param addr Which address to use for the measure operation. - */ - int _measure(unsigned addr); - - int reg_read(uint8_t reg, void *data, unsigned count = 1); - int reg_write(uint8_t reg, uint8_t data); - int reset(); - -}; - -device::Device * -MPL3115A2_i2c_interface(uint8_t busnum) -{ - return new MPL3115A2_I2C(busnum); -} - -MPL3115A2_I2C::MPL3115A2_I2C(uint8_t bus) : - I2C("MPL3115A2_I2C", nullptr, bus, 0, 400000) -{ -} - -int -MPL3115A2_I2C::init() -{ - /* this will call probe() */ - set_device_address(MPL3115A2_ADDRESS); - return I2C::init(); -} - -int -MPL3115A2_I2C::reset() -{ - int max = 10; - reg_write(MPL3115A2_CTRL_REG1, CTRL_REG1_RST); - int rv = CTRL_REG1_RST; - int ret = 1; - - while (ret == 1 && (rv & CTRL_REG1_RST) && max--) { - usleep(4000); - ret = reg_read(MPL3115A2_CTRL_REG1, &rv); - } - - return ret == 1 ? PX4_OK : ret; -} - -int -MPL3115A2_I2C::read(unsigned offset, void *data, unsigned count) -{ - - int ret = -EINVAL; - - switch (offset) { - case MPL3115A2_CTRL_REG1: - ret = reg_read(offset, data, count); - break; - - case OUT_P_MSB: { - union _cvt { - MPL3115A2_data_t reading; - } *cvt = (_cvt *)data; - - /* read the most recent measurement - * 3 Pressure and 2 temprtture - */ - uint8_t b[3 + 2]; - uint8_t reg = (uint8_t) offset; - - ret = transfer(®, 1, &b[0], sizeof(b)); - - if (ret == PX4_OK) { - cvt->reading.pressure.q = ((uint32_t)b[0]) << 18 | ((uint32_t) b[1]) << 10 | (((uint32_t)b[2]) & 0xc0) << 2 | (( - b[2] & 0x30) >> 4); - cvt->reading.temperature.w = ((uint16_t) b[3]) << 8 | (b[4] >> 4); - - } - } - break; - } - - return ret; -} - -int -MPL3115A2_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - case IOCTL_RESET: - ret = reset(); - break; - - case IOCTL_MEASURE: - ret = _measure(arg); - break; - - default: - ret = EINVAL; - } - - return ret; -} - -int -MPL3115A2_I2C::probe() -{ - _retries = 10; - uint8_t whoami = 0; - - if ((reg_read(MPL3115A2_REG_WHO_AM_I, &whoami) > 0) && (whoami == MPL3115A2_WHO_AM_I)) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return PX4_OK; - } - - return -EIO; -} - -int -MPL3115A2_I2C::_measure(unsigned addr) -{ - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - return reg_write((addr >> 8) & 0xff, addr & 0xff); -} - - -int MPL3115A2_I2C::reg_read(uint8_t reg, void *data, unsigned count) -{ - uint8_t cmd = reg; - int ret = transfer(&cmd, 1, (uint8_t *)data, count); - return ret == PX4_OK ? count : ret; -} - -int MPL3115A2_I2C::reg_write(uint8_t reg, uint8_t data) -{ - uint8_t buf[2] = { reg, data}; - int ret = transfer(buf, sizeof(buf), NULL, 0); - return ret == PX4_OK ? 2 : ret; -} diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp new file mode 100644 index 0000000000..ebd6a3dd60 --- /dev/null +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MPL3115A2.hpp" + +#include + +enum class MPL3115A2_BUS { + ALL = 0, + I2C_INTERNAL, + I2C_EXTERNAL, +}; + +namespace mpl3115a2 +{ + +struct mpl3115a2_bus_option { + MPL3115A2_BUS busid; + uint8_t busnum; + MPL3115A2 *dev; +} bus_options[] = { +#if defined(PX4_I2C_BUS_ONBOARD) + { MPL3115A2_BUS::I2C_INTERNAL, PX4_I2C_BUS_ONBOARD, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION) + { MPL3115A2_BUS::I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION1) + { MPL3115A2_BUS::I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION1, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION2) + { MPL3115A2_BUS::I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION2, nullptr }, +#endif +}; + +// find a bus structure for a busid +static mpl3115a2_bus_option *find_bus(MPL3115A2_BUS busid) +{ + for (mpl3115a2_bus_option &bus_option : bus_options) { + if ((busid == MPL3115A2_BUS::ALL || + busid == bus_option.busid) && bus_option.dev != nullptr) { + + return &bus_option; + } + } + + return nullptr; +} + +static bool start_bus(mpl3115a2_bus_option &bus) +{ + MPL3115A2 *dev = new MPL3115A2(bus.busnum); + + if (dev == nullptr || (dev->init() != PX4_OK)) { + PX4_ERR("driver start failed"); + delete dev; + return false; + } + + bus.dev = dev; + + return true; +} + +static int start(MPL3115A2_BUS busid) +{ + for (mpl3115a2_bus_option &bus_option : bus_options) { + if (bus_option.dev != nullptr) { + // this device is already started + PX4_WARN("already started"); + continue; + } + + if (busid != MPL3115A2_BUS::ALL && bus_option.busid != busid) { + // not the one that is asked for + continue; + } + + if (start_bus(bus_option)) { + return PX4_OK; + } + } + + return PX4_ERROR; +} + +static int stop(MPL3115A2_BUS busid) +{ + mpl3115a2_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 PX4_OK; +} + +static int status(MPL3115A2_BUS busid) +{ + mpl3115a2_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; +} + +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)"); + + return 0; +} + +} // namespace + +extern "C" int mpl3115a2_main(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + MPL3115A2_BUS busid = MPL3115A2_BUS::ALL; + + while ((ch = px4_getopt(argc, argv, "XI", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + busid = MPL3115A2_BUS::I2C_EXTERNAL; + break; + + case 'I': + busid = MPL3115A2_BUS::I2C_INTERNAL; + break; + + default: + return mpl3115a2::usage(); + } + } + + if (myoptind >= argc) { + return mpl3115a2::usage(); + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return mpl3115a2::start(busid); + + } else if (!strcmp(verb, "stop")) { + return mpl3115a2::stop(busid); + + } else if (!strcmp(verb, "status")) { + return mpl3115a2::status(busid); + } + + return mpl3115a2::usage(); +}