diff --git a/src/drivers/mpl3115a2/CMakeLists.txt b/src/drivers/mpl3115a2/CMakeLists.txt new file mode 100644 index 00000000000..43480945c7d --- /dev/null +++ b/src/drivers/mpl3115a2/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 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. +# +############################################################################ +set(srcs + mpl3115a2_i2c.cpp + mpl3115a2.cpp +) + +px4_add_module( + MODULE drivers__mpl3115a2 + MAIN mpl3115a2 + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS ${srcs} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mpl3115a2/mpl3115a2.cpp b/src/drivers/mpl3115a2/mpl3115a2.cpp new file mode 100644 index 00000000000..5d7fb222017 --- /dev/null +++ b/src/drivers/mpl3115a2/mpl3115a2.cpp @@ -0,0 +1,1229 @@ +/**************************************************************************** + * + * 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.cpp + * + * FIXME!: This is stubberd out driver for the NXP MPL3115A2 + * it has bogus code in it and is just being used to verify + * the i2C buss and WHOAMI + * + * Driver for the MPL3115A2 barometric pressure sensor connected via I2C. + */ + +#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 "mpl3115a2.h" + + + +enum MPL3115A2_BUS { + MPL3115A2_BUS_ALL = 0, + MPL3115A2_BUS_I2C_INTERNAL, + MPL3115A2_BUS_I2C_EXTERNAL, +}; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/* 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. + */ + +/* + * Maximum internal conversion time for TBD + */ +#define MPL3115A2_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MPL3115A2_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MPL3115A2_BARO_DEVICE_PATH_EXT "/dev/mpl3115a2_ext" +#define MPL3115A2_BARO_DEVICE_PATH_INT "/dev/mpl3115a2_int" + +class MPL3115A2 : public device::CDev +{ +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 *_interface; + + struct work_s _work; + unsigned _measure_ticks; + + ringbuffer::RingBuffer *_reports; + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate TBD! temperature values per MPL3115A2 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + float _P; + float _T; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in Pa */ + + 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_ticks the number of queue ticks 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_cycle(unsigned delay_ticks = 1); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * 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 cycle(); + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * 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("MPL3115A2", path), + _interface(interface), + _measure_ticks(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _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")) +{ + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + + // set the device type from the interface + _device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _device_id.devid_s.bus = _interface->get_device_bus(); + _device_id.devid_s.address = _interface->get_device_address(); + _device_id.devid_s.devtype = DRV_BARO_DEVTYPE_MPL3115A2; +} + +MPL3115A2::~MPL3115A2() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + 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) { + DEVICE_DEBUG("CDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); + + if (_reports == nullptr) { + DEVICE_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); + + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + while (true) { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MPL3115A2_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MPL3115A2_CONVERSION_INTERVAL); + + if (OK != collect()) { + 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 = _device_id.devid; + + ret = OK; + + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_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(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 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 { + _measure_phase = 0; + _reports->flush(); + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MPL3115A2_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MPL3115A2_CONVERSION_INTERVAL); + + 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) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MPL3115A2_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start_cycle(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start_cycle(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + 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. + */ + return OK; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); +} + +void +MPL3115A2::start_cycle(unsigned delay_ticks) +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MPL3115A2::cycle_trampoline, this, delay_ticks); +} + +void +MPL3115A2::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MPL3115A2::cycle_trampoline(void *arg) +{ + MPL3115A2 *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +MPL3115A2::cycle() +{ + int ret; + unsigned dummy; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (ret != OK) { + if (ret == -6) { + /* + * The mpl3115a2 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //DEVICE_LOG("collection error %d", ret); + } + + /* 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_cycle(USEC2TICK(2800)); + return; + } + + /* 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_ticks > USEC2TICK(MPL3115A2_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MPL3115A2::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (ret != OK) { + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MPL3115A2::cycle_trampoline, + this, + USEC2TICK(MPL3115A2_CONVERSION_INTERVAL)); +} + +int +MPL3115A2::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase TBD + */ + unsigned addr = 0; + + /* + * Send the command to begin measuring. + */ + ret = _interface->ioctl(IOCTL_MEASURE, addr); + + if (OK != ret) { + perf_count(_comms_errors); + } + + perf_end(_measure_perf); + + return ret; +} + +int +MPL3115A2::collect() +{ + int ret; + uint32_t raw; + + perf_begin(_sample_perf); + + struct baro_report 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(0, (void *)&raw, 0); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw; + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * 1) >> 23); + + /* base sensor scale/offset values */ + + /* Perform MPL3115A2 Caculation */ + + _OFF = ((int64_t) 1 << 16) + (((int64_t)1 * dT) >> 7); + _SENS = ((int64_t)1 << 15) + (((int64_t)1 * dT) >> 8); + + /* MPL3115A2 temperature compensation */ + + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; + + /* generate a new report */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ + + /* return device ID */ + report.device_id = _device_id.devid; + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: mpl3115a2_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: mpl3115a2_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + if (!(_pub_blocked) && _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); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MPL3115A2_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void +MPL3115A2::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + printf("device: mpl3115a2\n"); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); +} + +/** + * 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 +}; +#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 calibrate(unsigned altitude, enum MPL3115A2_BUS busid); +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); + struct baro_report 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"); + } + + warnx("single read"); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + errx(1, "failed to set queue depth"); + } + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* 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"); + } + + warnx("periodic read %u", i); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + } + + 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); +} + +/** + * Calculate actual MSL pressure given current altitude + */ +void +calibrate(unsigned altitude, enum MPL3115A2_BUS busid) +{ + struct mpl3115a2_bus_option &bus = find_bus(busid); + struct baro_report report; + float pressure; + float p1; + + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + err(1, "open failed (try 'mpl3115a2 start' if the driver is not running)"); + } + + /* start the sensor polling at max */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + errx(1, "failed to set poll rate"); + } + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + 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, "sensor read failed"); + } + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", (double)p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + err(1, "BAROIOCSMSLPRESSURE"); + } + + close(fd); + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); + warnx(" -T 5611|5607 (default 5611)"); + warnx(" -T 0 (autodetect version)"); + +} + +} // namespace + +int +mpl3115a2_main(int argc, char *argv[]) +{ + enum MPL3115A2_BUS busid = MPL3115A2_BUS_ALL; + int ch; + int myoptind = 1; + 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); + } + } + + 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(); + } + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + if (!strcmp(verb, "calibrate")) { + if (argc < 2) { + errx(1, "missing altitude"); + } + + long altitude = strtol(argv[optind + 1], nullptr, 10); + + mpl3115a2::calibrate(altitude, busid); + } + + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/mpl3115a2/mpl3115a2.h b/src/drivers/mpl3115a2/mpl3115a2.h new file mode 100644 index 00000000000..b354bb4f636 --- /dev/null +++ b/src/drivers/mpl3115a2/mpl3115a2.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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.h + * + * Shared defines for the mpl3115a2 driver. + */ + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +/* 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); diff --git a/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp new file mode 100644 index 00000000000..8ab0f168fdd --- /dev/null +++ b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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 + */ + +/* XXX trim includes */ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "mpl3115a2.h" + +#include "board_config.h" + +#define MPL3115A2_ADDRESS 0x60 +#define MPL3115A2_REG_WHO_AM_I 0x0c +#define MPL3115A2_WHO_AM_I 0xC4 + +device::Device *MPL3115A2_i2c_interface(uint8_t busnum); + +class MPL3115A2_I2C : public device::I2C +{ +public: + MPL3115A2_I2C(uint8_t bus); + virtual ~MPL3115A2_I2C(); + + 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(unsigned reg, void *data, unsigned count); + +}; + +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) +{ +} + +MPL3115A2_I2C::~MPL3115A2_I2C() +{ +} + +int +MPL3115A2_I2C::init() +{ + /* this will call probe() */ + set_address(MPL3115A2_ADDRESS); + return I2C::init(); +} + +int +MPL3115A2_I2C::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MPL3115A2_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + PX4_ERR("Not implemented"); + ret = EINVAL; + 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, 1) > 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; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + + +int MPL3115A2_I2C::reg_read(unsigned reg, void *data, unsigned count) +{ + uint8_t cmd = reg; + int ret = transfer(&cmd, 1, (uint8_t *)data, count); + return ret == PX4_OK ? count : ret; +}