From bcfa2eecd8eed1e13e9efd9ecd81f5eca3e2c194 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 13 Nov 2019 13:01:00 -0500 Subject: [PATCH] ms5611: build on linux (navio2) --- boards/emlid/navio2/cross.cmake | 1 + boards/emlid/navio2/native.cmake | 1 + src/drivers/barometer/ms5611/CMakeLists.txt | 2 + src/drivers/barometer/ms5611/MS5611.hpp | 177 +++++++ src/drivers/barometer/ms5611/ms5611.cpp | 490 +------------------ src/drivers/barometer/ms5611/ms5611_main.cpp | 277 +++++++++++ 6 files changed, 463 insertions(+), 485 deletions(-) create mode 100644 src/drivers/barometer/ms5611/MS5611.hpp create mode 100644 src/drivers/barometer/ms5611/ms5611_main.cpp diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index bd9c9ee19c..992e3b4040 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -10,6 +10,7 @@ px4_add_board( DRIVERS #barometer # all available barometer drivers + barometer/ms5611 batt_smbus camera_trigger differential_pressure # all available differential pressure drivers diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index de6a14d190..c4c054fb5a 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -8,6 +8,7 @@ px4_add_board( DRIVERS #barometer # all available barometer drivers + barometer/ms5611 batt_smbus camera_trigger differential_pressure # all available differential pressure drivers diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index 24e2d1382a..1e484eb5f8 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -40,6 +40,8 @@ px4_add_module( ms5611_spi.cpp ms5611_i2c.cpp ms5611.cpp + ms5611_main.cpp + MS5611.hpp DEPENDS px4_work_queue ) diff --git a/src/drivers/barometer/ms5611/MS5611.hpp b/src/drivers/barometer/ms5611/MS5611.hpp new file mode 100644 index 0000000000..224e47bca5 --- /dev/null +++ b/src/drivers/barometer/ms5611/MS5611.hpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ms5611.h" + +enum MS56XX_DEVICE_TYPES { + MS56XX_DEVICE = 0, + MS5611_DEVICE = 5611, + MS5607_DEVICE = 5607, +}; + +/* 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)) + +/* + * MS5611/MS5607 internal constants and data structures. + */ +#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ + +/* + use an OSR of 1024 to reduce the self-heating effect of the + sensor. Information from MS tells us that some individual sensors + are quite sensitive to this effect and that reducing the OSR can + make a big difference + */ +#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 +#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 + +/* + * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update + * rate of 100Hz which is be very safe not to read the ADC before the + * conversion finished + */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" + +class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem +{ +public: + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); + ~MS5611(); + + virtual int init(); + + virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + device::Device *_interface; + + ms5611::prom_s _prom; + + unsigned _measure_interval{0}; + + ringbuffer::RingBuffer *_reports; + enum MS56XX_DEVICE_TYPES _device_type; + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611/MS5607 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + 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. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * 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(); +}; diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 720b38e301..8549c8f07a 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -36,149 +36,10 @@ * Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI. */ +#include "MS5611.hpp" #include "ms5611.h" -enum MS56XX_DEVICE_TYPES { - MS56XX_DEVICE = 0, - MS5611_DEVICE = 5611, - MS5607_DEVICE = 5607, -}; - -enum MS5611_BUS { - MS5611_BUS_ALL = 0, - MS5611_BUS_I2C_INTERNAL, - MS5611_BUS_I2C_EXTERNAL, - MS5611_BUS_SPI_INTERNAL, - MS5611_BUS_SPI_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)) - -/* - * MS5611/MS5607 internal constants and data structures. - */ -#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ - -/* - use an OSR of 1024 to reduce the self-heating effect of the - sensor. Information from MS tells us that some individual sensors - are quite sensitive to this effect and that reducing the OSR can - make a big difference - */ -#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 -#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 - -/* - * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update - * rate of 100Hz which is be very safe not to read the ADC before the - * conversion finished - */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" -#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" - -class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem -{ -public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type); - ~MS5611(); - - 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; - - ms5611::prom_s _prom; - - unsigned _measure_interval{0}; - - ringbuffer::RingBuffer *_reports; - enum MS56XX_DEVICE_TYPES _device_type; - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611/MS5607 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - 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. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * 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 ms5611_main(int argc, char *argv[]); +#include MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type) : @@ -342,7 +203,7 @@ out: } ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) +MS5611::read(cdev::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(sensor_baro_s); sensor_baro_s *brp = reinterpret_cast(buffer); @@ -414,7 +275,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } int -MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +MS5611::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -740,45 +601,6 @@ MS5611::print_info() namespace ms5611 { -/* - list of supported bus configurations - */ -struct ms5611_bus_option { - enum MS5611_BUS busid; - const char *devpath; - MS5611_constructor interface_constructor; - uint8_t busnum; - MS5611 *dev; -} bus_options[] = { -#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) - { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, -#endif -#ifdef PX4_SPIDEV_BARO - { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_BARO, NULL }, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION - { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION1 - { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext1", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION2 - { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext2", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type); -struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); -void start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type); -void test(enum MS5611_BUS busid); -void reset(enum MS5611_BUS busid); -void info(); -void usage(); - /** * MS5611 crc4 cribbed from the datasheet */ @@ -825,306 +647,4 @@ crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } - -/** - * Start the driver. - */ -bool -start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) -{ - if (bus.dev != nullptr) { - errx(1, "bus option already started"); - } - - prom_u prom_buf; - device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); - - if (interface->init() != OK) { - delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type); - - 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 MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) -{ - uint8_t i; - bool started = false; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started = started | start_bus(bus_options[i], device_type); - } - - if (!started) { - exit(1); - } - - // one or more drivers started OK - exit(0); -} - - -/** - * find a bus structure for a busid - */ -struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == MS5611_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 MS5611_BUS busid) -{ - struct ms5611_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 'ms5611 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 MS5611_BUS busid) -{ - struct ms5611_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 ms5611_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)"); - warnx(" -S (external SPI bus)"); - warnx(" -s (internal SPI bus)"); - warnx(" -T 5611|5607 (default 5611)"); - warnx(" -T 0 (autodetect version)"); - -} - -} // namespace - -int -ms5611_main(int argc, char *argv[]) -{ - enum MS5611_BUS busid = MS5611_BUS_ALL; - enum MS56XX_DEVICE_TYPES device_type = MS5611_DEVICE; - 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, "T:XISs", &myoptind, &myoptarg)) != 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); - - if (val == 5611) { - device_type = MS5611_DEVICE; - break; - - } else if (val == 5607) { - device_type = MS5607_DEVICE; - break; - - } else if (val == 0) { - device_type = MS56XX_DEVICE; - break; - } - } - - /* FALLTHROUGH */ - - default: - ms5611::usage(); - return 0; - } - } - - if (myoptind >= argc) { - ms5611::usage(); - return -1; - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ms5611::start(busid, device_type); - } - - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - ms5611::test(busid); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - ms5611::reset(busid); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - ms5611::info(); - } - - PX4_ERR("unrecognised command, try 'start', 'test', 'reset' or 'info'"); - return -1; -} +} // namespace ms5611 diff --git a/src/drivers/barometer/ms5611/ms5611_main.cpp b/src/drivers/barometer/ms5611/ms5611_main.cpp new file mode 100644 index 0000000000..42aac3a309 --- /dev/null +++ b/src/drivers/barometer/ms5611/ms5611_main.cpp @@ -0,0 +1,277 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file ms5611.cpp + * Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI. + */ + +#include "MS5611.hpp" +#include "ms5611.h" + +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); + +/** + * Local functions in support of the shell command. + */ +namespace ms5611 +{ + +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_BARO, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION1 + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext1", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION1, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION2 + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext2", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION2, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +/** + * Start the driver. + */ +static bool +start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type) +{ + if (bus.dev != nullptr) { + PX4_ERR("bus option already started"); + return false; + } + + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + + if (interface->init() != OK) { + delete interface; + PX4_WARN("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type); + + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = px4_open(bus.devpath, O_RDONLY); + + /* set the poll rate to default, starts automatic data collection */ + if (fd == -1) { + PX4_ERR("can't open baro device"); + return false; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + px4_close(fd); + PX4_ERR("failed setting default poll rate"); + return false; + } + + px4_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. + */ +static int +start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type) +{ + uint8_t i; + bool started = false; + + for (i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started = started | start_bus(bus_options[i], device_type); + } + + if (!started) { + return PX4_ERROR; + } + + // one or more drivers started OK + return PX4_OK; +} + +static int +info() +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + struct ms5611_bus_option &bus = bus_options[i]; + + if (bus.dev != nullptr) { + PX4_INFO("%s", bus.devpath); + bus.dev->print_info(); + } + } + + return 0; +} + +static int +usage() +{ + PX4_INFO("missing command: try 'start', 'info',"); + PX4_INFO("options:"); + PX4_INFO(" -X (external I2C bus)"); + PX4_INFO(" -I (intternal I2C bus)"); + PX4_INFO(" -S (external SPI bus)"); + PX4_INFO(" -s (internal SPI bus)"); + PX4_INFO(" -T 5611|5607 (default 5611)"); + PX4_INFO(" -T 0 (autodetect version)"); + + return 0; +} + +} // namespace + +int +ms5611_main(int argc, char *argv[]) +{ + enum MS5611_BUS busid = MS5611_BUS_ALL; + enum MS56XX_DEVICE_TYPES device_type = MS5611_DEVICE; + 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, "T:XISs", &myoptind, &myoptarg)) != 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); + + if (val == 5611) { + device_type = MS5611_DEVICE; + break; + + } else if (val == 5607) { + device_type = MS5607_DEVICE; + break; + + } else if (val == 0) { + device_type = MS56XX_DEVICE; + break; + } + } + + /* FALLTHROUGH */ + + default: + return ms5611::usage(); + } + } + + if (myoptind >= argc) { + return ms5611::usage(); + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + return ms5611::start(busid, device_type); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + return ms5611::info(); + } + + return ms5611::usage(); +}