From 1da7209fe1d6c5e05d3863a53a7cbacb8d50d28c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 3 Jan 2020 15:24:54 -0500 Subject: [PATCH] lps25h: move to PX4Barometer and cleanup --- src/drivers/barometer/lps25h/CMakeLists.txt | 13 +- src/drivers/barometer/lps25h/LPS25H.cpp | 212 ++++ src/drivers/barometer/lps25h/LPS25H.hpp | 184 ++++ src/drivers/barometer/lps25h/lps25h.cpp | 968 ------------------- src/drivers/barometer/lps25h/lps25h.h | 6 - src/drivers/barometer/lps25h/lps25h_i2c.cpp | 41 +- src/drivers/barometer/lps25h/lps25h_main.cpp | 227 +++++ src/drivers/barometer/lps25h/lps25h_spi.cpp | 61 +- 8 files changed, 659 insertions(+), 1053 deletions(-) create mode 100644 src/drivers/barometer/lps25h/LPS25H.cpp create mode 100644 src/drivers/barometer/lps25h/LPS25H.hpp delete mode 100644 src/drivers/barometer/lps25h/lps25h.cpp create mode 100644 src/drivers/barometer/lps25h/lps25h_main.cpp diff --git a/src/drivers/barometer/lps25h/CMakeLists.txt b/src/drivers/barometer/lps25h/CMakeLists.txt index 457f69aa2f..778221bd41 100644 --- a/src/drivers/barometer/lps25h/CMakeLists.txt +++ b/src/drivers/barometer/lps25h/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2020 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,12 +31,15 @@ # ############################################################################ px4_add_module( - MODULE drivers__lps25h + MODULE drivers__barometer__lps25h MAIN lps25h - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS - lps25h.cpp + LPS25H.cpp + LPS25H.hpp + lps25h_main.cpp lps25h_i2c.cpp lps25h_spi.cpp + DEPENDS + drivers_barometer + px4_work_queue ) diff --git a/src/drivers/barometer/lps25h/LPS25H.cpp b/src/drivers/barometer/lps25h/LPS25H.cpp new file mode 100644 index 0000000000..1ae31c9801 --- /dev/null +++ b/src/drivers/barometer/lps25h/LPS25H.cpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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 "LPS25H.hpp" + +LPS25H::LPS25H(device::Device *interface) : + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), + _px4_barometer(interface->get_device_id()), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")) +{ + _interface->set_device_type(DRV_BARO_DEVTYPE_LPS25H); + _px4_barometer.set_device_type(DRV_BARO_DEVTYPE_LPS25H); +} + +LPS25H::~LPS25H() +{ + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); + + delete _interface; +} + +int LPS25H::init() +{ + if (reset() != OK) { + return PX4_ERROR; + } + + start(); + + return PX4_OK; +} + +void LPS25H::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + ScheduleNow(); +} + +void LPS25H::stop() +{ + ScheduleClear(); +} + +int LPS25H::reset() +{ + // Power on + int ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD); + usleep(1000); + + // Reset + ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_BOOT | CTRL_REG2_SWRESET); + usleep(5000); + + // Power on + ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD); + usleep(1000); + + return ret; +} + +void LPS25H::Run() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + PX4_DEBUG("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_interval > (LPS25H_CONVERSION_INTERVAL)) { + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - LPS25H_CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + PX4_DEBUG("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(LPS25H_CONVERSION_INTERVAL); +} + +int LPS25H::measure() +{ + /* + * Send the command to begin a 16-bit measurement. + */ + int ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_ONE_SHOT); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int LPS25H::collect() +{ + perf_begin(_sample_perf); + + struct { + uint8_t status; + uint8_t p_xl; + uint8_t p_l; + uint8_t p_h; + int16_t t; + } report{}; + + /* get measurements from the device : MSB enables register address auto-increment */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = _interface->read(ADDR_STATUS_REG | (1 << 7), (uint8_t *)&report, sizeof(report)); + + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + _px4_barometer.set_error_count(perf_event_count(_comms_errors)); + + /* get measurements from the device */ + float temperature = 42.5f + (report.t / 480); + _px4_barometer.set_temperature(temperature); + + /* raw pressure */ + uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16); + + /* Pressure and MSL in mBar */ + float pressure = raw / 4096.0f; + + _px4_barometer.update(timestamp_sample, pressure); + + perf_end(_sample_perf); + return PX4_OK; +} + +int LPS25H::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf = val; + return _interface->write(reg, &buf, 1); +} + +int LPS25H::read_reg(uint8_t reg, uint8_t &val) +{ + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; +} + +void LPS25H::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + _px4_barometer.print_status(); +} diff --git a/src/drivers/barometer/lps25h/LPS25H.hpp b/src/drivers/barometer/lps25h/LPS25H.hpp new file mode 100644 index 0000000000..e000ffb089 --- /dev/null +++ b/src/drivers/barometer/lps25h/LPS25H.hpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 lps25h.cpp + * + * Driver for the LPS25H barometer connected via I2C or SPI. + */ + +#pragma once + +#include "lps25h.h" + +#include +#include +#include +#include + +/* + * LPS25H internal constants and data structures. + */ + +/* Max measurement rate is 25Hz */ +#define LPS25H_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */ + +#define ADDR_REF_P_XL 0x08 +#define ADDR_REF_P_L 0x09 +#define ADDR_REF_P_H 0x0A +#define ADDR_WHO_AM_I 0x0F +#define ADDR_RES_CONF 0x10 +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_INT_CFG 0x24 +#define ADDR_INT_SOURCE 0x25 + +#define ADDR_STATUS_REG 0x27 +#define ADDR_P_OUT_XL 0x28 +#define ADDR_P_OUT_L 0x29 +#define ADDR_P_OUT_H 0x2A +#define ADDR_TEMP_OUT_L 0x2B +#define ADDR_TEMP_OUT_H 0x2C + +#define ADDR_FIFO_CTRL 0x2E +#define ADDR_FIFO_STATUS 0x2F +#define ADDR_THS_P_L 0x30 +#define ADDR_THS_P_H 0x31 + +#define ADDR_RPDS_L 0x39 +#define ADDR_RPDS_H 0x3A + +/* Data sheet is ambigious if AVGT or AVGP is first */ +#define RES_CONF_AVGT_8 0x00 +#define RES_CONF_AVGT_32 0x01 +#define RES_CONF_AVGT_128 0x02 +#define RES_CONF_AVGT_512 0x03 +#define RES_CONF_AVGP_8 0x00 +#define RES_CONF_AVGP_32 0x04 +#define RES_CONF_AVGP_128 0x08 +#define RES_CONF_AVGP_512 0x0C + +#define CTRL_REG1_SIM (1 << 0) +#define CTRL_REG1_RESET_AZ (1 << 1) +#define CTRL_REG1_BDU (1 << 2) +#define CTRL_REG1_DIFF_EN (1 << 3) +#define CTRL_REG1_PD (1 << 7) +#define CTRL_REG1_ODR_SINGLE (0 << 4) +#define CTRL_REG1_ODR_1HZ (1 << 4) +#define CTRL_REG1_ODR_7HZ (2 << 4) +#define CTRL_REG1_ODR_12HZ5 (3 << 4) +#define CTRL_REG1_ODR_25HZ (4 << 4) + +#define CTRL_REG2_ONE_SHOT (1 << 0) +#define CTRL_REG2_AUTO_ZERO (1 << 1) +#define CTRL_REG2_SWRESET (1 << 2) +#define CTRL_REG2_FIFO_MEAN_DEC (1 << 4) +#define CTRL_REG2_WTM_EN (1 << 5) +#define CTRL_REG2_FIFO_EN (1 << 6) +#define CTRL_REG2_BOOT (1 << 7) + +#define CTRL_REG3_INT1_S_DATA 0x0 +#define CTRL_REG3_INT1_S_P_HIGH 0x1 +#define CTRL_REG3_INT1_S_P_LOW 0x2 +#define CTRL_REG3_INT1_S_P_LIM 0x3 +#define CTRL_REG3_PP_OD (1 << 6) +#define CTRL_REG3_INT_H_L (1 << 7) + +#define CTRL_REG4_P1_DRDY (1 << 0) +#define CTRL_REG4_P1_OVERRUN (1 << 1) +#define CTRL_REG4_P1_WTM (1 << 2) +#define CTRL_REG4_P1_EMPTY (1 << 3) + +#define INTERRUPT_CFG_PH_E (1 << 0) +#define INTERRUPT_CFG_PL_E (1 << 1) +#define INTERRUPT_CFG_LIR (1 << 2) + +#define INT_SOURCE_PH (1 << 0) +#define INT_SOURCE_PL (1 << 1) +#define INT_SOURCE_IA (1 << 2) + +#define STATUS_REG_T_DA (1 << 0) +#define STATUS_REG_P_DA (1 << 1) +#define STATUS_REG_T_OR (1 << 4) +#define STATUS_REG_P_OR (1 << 5) + +#define FIFO_CTRL_WTM_FMEAN_2 0x01 +#define FIFO_CTRL_WTM_FMEAN_4 0x03 +#define FIFO_CTRL_WTM_FMEAN_8 0x07 +#define FIFO_CTRL_WTM_FMEAN_16 0x0F +#define FIFO_CTRL_WTM_FMEAN_32 0x1F +#define FIFO_CTRL_F_MODE_BYPASS (0x0 << 5) +#define FIFO_CTRL_F_MODE_FIFO (0x1 << 5) +#define FIFO_CTRL_F_MODE_STREAM (0x2 << 5) +#define FIFO_CTRL_F_MODE_SFIFO (0x3 << 5) +#define FIFO_CTRL_F_MODE_BSTRM (0x4 << 5) +#define FIFO_CTRL_F_MODE_FMEAN (0x6 << 5) +#define FIFO_CTRL_F_MODE_BFIFO (0x7 << 5) + +#define FIFO_STATUS_EMPTY (1 << 5) +#define FIFO_STATUS_FULL (1 << 6) +#define FIFO_STATUS_WTM (1 << 7) + +class LPS25H : public px4::ScheduledWorkItem +{ +public: + LPS25H(device::Device *interface); + ~LPS25H() override; + + int init(); + void print_info(); + +private: + + void start(); + void stop(); + int reset(); + void Run() override; + + int write_reg(uint8_t reg, uint8_t val); + int read_reg(uint8_t reg, uint8_t &val); + + int measure(); + int collect(); + + PX4Barometer _px4_barometer; + device::Device *_interface; + + unsigned _measure_interval{0}; + bool _collect_phase{false}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; +}; diff --git a/src/drivers/barometer/lps25h/lps25h.cpp b/src/drivers/barometer/lps25h/lps25h.cpp deleted file mode 100644 index 55b57d2bf8..0000000000 --- a/src/drivers/barometer/lps25h/lps25h.cpp +++ /dev/null @@ -1,968 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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 lps25h.cpp - * - * Driver for the LPS25H barometer connected via I2C or SPI. - */ - -#include "lps25h.h" - -/* - * LPS25H internal constants and data structures. - */ - -/* Max measurement rate is 25Hz */ -#define LPS25H_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */ - -#define ADDR_REF_P_XL 0x08 -#define ADDR_REF_P_L 0x09 -#define ADDR_REF_P_H 0x0A -#define ADDR_WHO_AM_I 0x0F -#define ADDR_RES_CONF 0x10 -#define ADDR_CTRL_REG1 0x20 -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define ADDR_INT_CFG 0x24 -#define ADDR_INT_SOURCE 0x25 - -#define ADDR_STATUS_REG 0x27 -#define ADDR_P_OUT_XL 0x28 -#define ADDR_P_OUT_L 0x29 -#define ADDR_P_OUT_H 0x2A -#define ADDR_TEMP_OUT_L 0x2B -#define ADDR_TEMP_OUT_H 0x2C - -#define ADDR_FIFO_CTRL 0x2E -#define ADDR_FIFO_STATUS 0x2F -#define ADDR_THS_P_L 0x30 -#define ADDR_THS_P_H 0x31 - -#define ADDR_RPDS_L 0x39 -#define ADDR_RPDS_H 0x3A - -/* Data sheet is ambigious if AVGT or AVGP is first */ -#define RES_CONF_AVGT_8 0x00 -#define RES_CONF_AVGT_32 0x01 -#define RES_CONF_AVGT_128 0x02 -#define RES_CONF_AVGT_512 0x03 -#define RES_CONF_AVGP_8 0x00 -#define RES_CONF_AVGP_32 0x04 -#define RES_CONF_AVGP_128 0x08 -#define RES_CONF_AVGP_512 0x0C - -#define CTRL_REG1_SIM (1 << 0) -#define CTRL_REG1_RESET_AZ (1 << 1) -#define CTRL_REG1_BDU (1 << 2) -#define CTRL_REG1_DIFF_EN (1 << 3) -#define CTRL_REG1_PD (1 << 7) -#define CTRL_REG1_ODR_SINGLE (0 << 4) -#define CTRL_REG1_ODR_1HZ (1 << 4) -#define CTRL_REG1_ODR_7HZ (2 << 4) -#define CTRL_REG1_ODR_12HZ5 (3 << 4) -#define CTRL_REG1_ODR_25HZ (4 << 4) - -#define CTRL_REG2_ONE_SHOT (1 << 0) -#define CTRL_REG2_AUTO_ZERO (1 << 1) -#define CTRL_REG2_SWRESET (1 << 2) -#define CTRL_REG2_FIFO_MEAN_DEC (1 << 4) -#define CTRL_REG2_WTM_EN (1 << 5) -#define CTRL_REG2_FIFO_EN (1 << 6) -#define CTRL_REG2_BOOT (1 << 7) - -#define CTRL_REG3_INT1_S_DATA 0x0 -#define CTRL_REG3_INT1_S_P_HIGH 0x1 -#define CTRL_REG3_INT1_S_P_LOW 0x2 -#define CTRL_REG3_INT1_S_P_LIM 0x3 -#define CTRL_REG3_PP_OD (1 << 6) -#define CTRL_REG3_INT_H_L (1 << 7) - -#define CTRL_REG4_P1_DRDY (1 << 0) -#define CTRL_REG4_P1_OVERRUN (1 << 1) -#define CTRL_REG4_P1_WTM (1 << 2) -#define CTRL_REG4_P1_EMPTY (1 << 3) - -#define INTERRUPT_CFG_PH_E (1 << 0) -#define INTERRUPT_CFG_PL_E (1 << 1) -#define INTERRUPT_CFG_LIR (1 << 2) - -#define INT_SOURCE_PH (1 << 0) -#define INT_SOURCE_PL (1 << 1) -#define INT_SOURCE_IA (1 << 2) - -#define STATUS_REG_T_DA (1 << 0) -#define STATUS_REG_P_DA (1 << 1) -#define STATUS_REG_T_OR (1 << 4) -#define STATUS_REG_P_OR (1 << 5) - -#define FIFO_CTRL_WTM_FMEAN_2 0x01 -#define FIFO_CTRL_WTM_FMEAN_4 0x03 -#define FIFO_CTRL_WTM_FMEAN_8 0x07 -#define FIFO_CTRL_WTM_FMEAN_16 0x0F -#define FIFO_CTRL_WTM_FMEAN_32 0x1F -#define FIFO_CTRL_F_MODE_BYPASS (0x0 << 5) -#define FIFO_CTRL_F_MODE_FIFO (0x1 << 5) -#define FIFO_CTRL_F_MODE_STREAM (0x2 << 5) -#define FIFO_CTRL_F_MODE_SFIFO (0x3 << 5) -#define FIFO_CTRL_F_MODE_BSTRM (0x4 << 5) -#define FIFO_CTRL_F_MODE_FMEAN (0x6 << 5) -#define FIFO_CTRL_F_MODE_BFIFO (0x7 << 5) - -#define FIFO_STATUS_EMPTY (1 << 5) -#define FIFO_STATUS_FULL (1 << 6) -#define FIFO_STATUS_WTM (1 << 7) - -enum LPS25H_BUS { - LPS25H_BUS_ALL = 0, - LPS25H_BUS_I2C_INTERNAL, - LPS25H_BUS_I2C_EXTERNAL, - LPS25H_BUS_SPI -}; - -class LPS25H : public cdev::CDev, public px4::ScheduledWorkItem -{ -public: - LPS25H(device::Device *interface, const char *path); - virtual ~LPS25H(); - - 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; - -private: - unsigned _measure_interval{0}; - - ringbuffer::RingBuffer *_reports{nullptr}; - bool _collect_phase{false}; - - orb_advert_t _baro_topic{nullptr}; - int _orb_class_instance{-1}; - int _class_instance{-1}; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - sensor_baro_s _last_report{}; /**< used for info() */ - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Reset the device - */ - int reset(); - - /** - * 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; - - /** - * Write a register. - * - * @param reg The register to write. - * @param val The value to write. - * @return OK on write success. - */ - int write_reg(uint8_t reg, uint8_t val); - - /** - * Read a register. - * - * @param reg The register to read. - * @param val The value read. - * @return OK on read success. - */ - int read_reg(uint8_t reg, uint8_t &val); - - /** - * Issue a measurement command. - * - * @return OK if the measurement command was successful. - */ - int measure(); - - /** - * Collect the result of the most recent measurement. - */ - int collect(); - - /* this class has pointer data members, do not allow copying it */ - LPS25H(const LPS25H &); - LPS25H operator=(const LPS25H &); -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int lps25h_main(int argc, char *argv[]); - - -LPS25H::LPS25H(device::Device *interface, const char *path) : - CDev(path), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), - _interface(interface), - _sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")), - _comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors")) -{ - _interface->set_device_type(DRV_BARO_DEVTYPE_LPS25H); -} - -LPS25H::~LPS25H() -{ - /* make sure we are truly inactive */ - stop(); - - if (_class_instance != -1) { - unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance); - } - - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - - delete _interface; -} - -int -LPS25H::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; - } - - if (reset() != OK) { - goto out; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - ret = OK; - -out: - return ret; -} - -ssize_t -LPS25H::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 */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(LPS25H_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - if (_reports->get(brp)) { - ret = sizeof(sensor_baro_s); - } - } while (0); - - return ret; -} - -int -LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - unsigned dummy = 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 = (LPS25H_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 < (LPS25H_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: - return reset(); - - case DEVIOCGDEVICEID: - return _interface->ioctl(cmd, dummy); - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -void -LPS25H::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -LPS25H::stop() -{ - ScheduleClear(); -} - -int -LPS25H::reset() -{ - int ret = 0; - - // Power on - ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD); - - usleep(1000); - - // Reset - ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_BOOT | CTRL_REG2_SWRESET); - - usleep(5000); - - // Power on - ret = write_reg(ADDR_CTRL_REG1, CTRL_REG1_PD); - - usleep(1000); - - return ret; -} - -void -LPS25H::Run() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - PX4_DEBUG("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > (LPS25H_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - LPS25H_CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - if (OK != measure()) { - PX4_DEBUG("measure error"); - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(LPS25H_CONVERSION_INTERVAL); -} - -int -LPS25H::measure() -{ - int ret; - - /* - * Send the command to begin a 16-bit measurement. - */ - ret = write_reg(ADDR_CTRL_REG2, CTRL_REG2_ONE_SHOT); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int -LPS25H::collect() -{ -#pragma pack(push, 1) - struct { - uint8_t status; - uint8_t p_xl, p_l, p_h; - int16_t t; - } report; -#pragma pack(pop) - - int ret; - - perf_begin(_sample_perf); - sensor_baro_s new_report; - bool sensor_is_onboard = false; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - new_report.timestamp = hrt_absolute_time(); - new_report.error_count = perf_event_count(_comms_errors); - - /* - * @note We could read the status register 1 here, which could tell us that - * we were too early and that the output registers are still being - * written. In the common case that would just slow us down, and - * we're better off just never being early. - */ - - /* get measurements from the device : MSB enables register address auto-increment */ - ret = _interface->read(ADDR_STATUS_REG | (1 << 7), (uint8_t *)&report, sizeof(report)); - - if (ret != OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - /* get measurements from the device */ - new_report.temperature = 42.5f + (report.t / 480); - - /* raw pressure */ - uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16); - - /* Pressure and MSL in mBar */ - float p = raw / 4096.0f; - - new_report.pressure = p; - - /* get device ID */ - new_report.device_id = _interface->get_device_id(); - - if (_baro_topic != nullptr) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report); - - } else { - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - - if (_baro_topic == nullptr) { - PX4_DEBUG("ADVERT FAIL"); - } - } - - _last_report = new_report; - - /* post a report to the ring */ - _reports->force(&new_report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - - ret = OK; - - perf_end(_sample_perf); - return ret; -} - -int -LPS25H::write_reg(uint8_t reg, uint8_t val) -{ - uint8_t buf = val; - return _interface->write(reg, &buf, 1); -} - -int -LPS25H::read_reg(uint8_t reg, uint8_t &val) -{ - uint8_t buf = val; - int ret = _interface->read(reg, &buf, 1); - val = buf; - return ret; -} - -void -LPS25H::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u \n", _measure_interval); - print_message(_last_report); - - _reports->print_info("report queue"); -} - -/** - * Local functions in support of the shell command. - */ -namespace lps25h -{ - -/* - list of supported bus configurations - */ -struct lps25h_bus_option { - enum LPS25H_BUS busid; - const char *devpath; - LPS25H_constructor interface_constructor; - uint8_t busnum; - LPS25H *dev; -} bus_options[] = { - { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, -#ifdef PX4_I2C_BUS_EXPANSION1 - { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext1", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION2 - { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext2", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL }, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { LPS25H_BUS_I2C_INTERNAL, "/dev/lps25h_int", &LPS25H_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, -#endif -#ifdef PX4_SPIDEV_HMC - { LPS25H_BUS_SPI, "/dev/lps25h_spi", &LPS25H_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -void start(enum LPS25H_BUS busid); -bool start_bus(struct lps25h_bus_option &bus); -struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid); -void test(enum LPS25H_BUS busid); -void reset(enum LPS25H_BUS busid); -void info(); -void usage(); - -/** - * start driver for a specific bus option - */ -bool -start_bus(struct lps25h_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 LPS25H(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 LPS25H_BUS busid) -{ - bool started = false; - - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == LPS25H_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (busid != LPS25H_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started |= start_bus(bus_options[i]); - } - - if (!started) { - exit(1); - } -} - -/** - * find a bus structure for a busid - */ -struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid) -{ - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == LPS25H_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 LPS25H_BUS busid) -{ - struct lps25h_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 'lps25h 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 LPS25H_BUS busid) -{ - struct lps25h_bus_option &bus = find_bus(busid); - const char *path = bus.devpath; - - int fd = open(path, 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 lps25h_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 (internal I2C bus)"); - warnx(" -S (external SPI bus)"); - warnx(" -s (internal SPI bus)"); -} - -} // namespace - -int -lps25h_main(int argc, char *argv[]) -{ - enum LPS25H_BUS busid = LPS25H_BUS_ALL; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "XIS:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { -#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) - - case 'I': - busid = LPS25H_BUS_I2C_INTERNAL; - break; -#endif - - case 'X': - busid = LPS25H_BUS_I2C_EXTERNAL; - break; - - case 'S': - busid = LPS25H_BUS_SPI; - break; - - default: - lps25h::usage(); - exit(0); - } - } - - if (myoptind >= argc) { - lps25h::usage(); - exit(0); - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - lps25h::start(busid); - } - - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - lps25h::test(busid); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - lps25h::reset(busid); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - lps25h::info(); - } - - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/src/drivers/barometer/lps25h/lps25h.h b/src/drivers/barometer/lps25h/lps25h.h index 7269fc1924..fc94784ccb 100644 --- a/src/drivers/barometer/lps25h/lps25h.h +++ b/src/drivers/barometer/lps25h/lps25h.h @@ -43,16 +43,10 @@ #include #include -#include #include -#include -#include -#include #include #include #include -#include - #define ADDR_WHO_AM_I 0x0F diff --git a/src/drivers/barometer/lps25h/lps25h_i2c.cpp b/src/drivers/barometer/lps25h/lps25h_i2c.cpp index 63459cd811..63f076dd9e 100644 --- a/src/drivers/barometer/lps25h/lps25h_i2c.cpp +++ b/src/drivers/barometer/lps25h/lps25h_i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2020 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 @@ -39,6 +39,8 @@ #include "lps25h.h" +#include + #define LPS25H_ADDRESS 0x5D device::Device *LPS25H_I2C_interface(int bus); @@ -47,20 +49,17 @@ class LPS25H_I2C : public device::I2C { public: LPS25H_I2C(int bus); - virtual ~LPS25H_I2C() = default; - - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); + virtual ~LPS25H_I2C() override = default; - virtual int ioctl(unsigned operation, unsigned &arg); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + int probe(); }; -device::Device * -LPS25H_I2C_interface(int bus) +device::Device *LPS25H_I2C_interface(int bus) { return new LPS25H_I2C(bus); } @@ -68,27 +67,10 @@ LPS25H_I2C_interface(int bus) LPS25H_I2C::LPS25H_I2C(int bus) : I2C("LPS25H_I2C", nullptr, bus, LPS25H_ADDRESS, 400000) { + set_device_type(DRV_BARO_DEVTYPE_LPS25H); } -int -LPS25H_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: - ret = -EINVAL; - } - - return ret; -} - -int -LPS25H_I2C::probe() +int LPS25H_I2C::probe() { uint8_t id; @@ -109,8 +91,7 @@ LPS25H_I2C::probe() return OK; } -int -LPS25H_I2C::write(unsigned address, void *data, unsigned count) +int LPS25H_I2C::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; diff --git a/src/drivers/barometer/lps25h/lps25h_main.cpp b/src/drivers/barometer/lps25h/lps25h_main.cpp new file mode 100644 index 0000000000..659b314c43 --- /dev/null +++ b/src/drivers/barometer/lps25h/lps25h_main.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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 +#include + +#include "LPS25H.hpp" + +enum class LPS25H_BUS { + ALL = 0, + I2C_INTERNAL, + I2C_EXTERNAL, + SPI_INTERNAL, + SPI_EXTERNAL +}; + +namespace lps25h +{ + +struct lps25h_bus_option { + LPS25H_BUS busid; + LPS25H_constructor interface_constructor; + uint8_t busnum; + LPS25H *dev; +} bus_options[] = { + { LPS25H_BUS::I2C_EXTERNAL, &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION, nullptr }, +#if defined(PX4_I2C_BUS_EXPANSION1) + { LPS25H_BUS::I2C_EXTERNAL, &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION1, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION2) + { LPS25H_BUS::I2C_EXTERNAL, &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION2, nullptr }, +#endif +#if defined(PX4_I2C_BUS_ONBOARD) + { LPS25H_BUS::I2C_INTERNAL, &LPS25H_I2C_interface, PX4_I2C_BUS_ONBOARD, nullptr }, +#endif +#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_LPS22H) + { LPS25H_BUS::SPI_EXTERNAL, &LPS25H_SPI_interface, PX4_SPI_BUS_SENSORS, nullptr }, +#endif +}; + +// find a bus structure for a busid +static struct lps25h_bus_option *find_bus(LPS25H_BUS busid) +{ + for (lps25h_bus_option &bus_option : bus_options) { + if ((busid == LPS25H_BUS::ALL || + busid == bus_option.busid) && bus_option.dev != nullptr) { + + return &bus_option; + } + } + + return nullptr; +} + +static bool start_bus(lps25h_bus_option &bus) +{ + device::Device *interface = bus.interface_constructor(bus.busnum); + + if (interface->init() != OK) { + PX4_WARN("no device on bus %u", (unsigned)bus.busid); + delete interface; + return false; + } + + LPS25H *dev = new LPS25H(interface); + + if (dev == nullptr) { + PX4_ERR("alloc failed"); + return false; + } + + if (dev->init() != PX4_OK) { + PX4_ERR("driver start failed"); + delete dev; + delete interface; + return false; + } + + bus.dev = dev; + + return true; +} + +static int start(LPS25H_BUS busid) +{ + for (lps25h_bus_option &bus_option : bus_options) { + if (bus_option.dev != nullptr) { + // this device is already started + PX4_WARN("already started"); + continue; + } + + if (busid != LPS25H_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(LPS25H_BUS busid) +{ + lps25h_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(LPS25H_BUS busid) +{ + lps25h_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)"); + PX4_INFO(" -s (spi internal bus)"); + PX4_INFO(" -S (spi external bus)"); + + return 0; +} + +} // namespace + +extern "C" int lps25h_main(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + LPS25H_BUS busid = LPS25H_BUS::ALL; + + while ((ch = px4_getopt(argc, argv, "XISs:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + busid = LPS25H_BUS::I2C_EXTERNAL; + break; + + case 'I': + busid = LPS25H_BUS::I2C_INTERNAL; + break; + + case 'S': + busid = LPS25H_BUS::SPI_EXTERNAL; + break; + + case 's': + busid = LPS25H_BUS::SPI_INTERNAL; + break; + + default: + return lps25h::usage(); + } + } + + if (myoptind >= argc) { + return lps25h::usage(); + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + lps25h::start(busid); + + } else if (!strcmp(verb, "stop")) { + return lps25h::stop(busid); + + } else if (!strcmp(verb, "status")) { + lps25h::status(busid); + } + + return lps25h::usage(); +} diff --git a/src/drivers/barometer/lps25h/lps25h_spi.cpp b/src/drivers/barometer/lps25h/lps25h_spi.cpp index 74328702be..289b53e19a 100644 --- a/src/drivers/barometer/lps25h/lps25h_spi.cpp +++ b/src/drivers/barometer/lps25h/lps25h_spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2020 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 @@ -39,49 +39,42 @@ #include "lps25h.h" -#ifdef PX4_SPIDEV_HMC +#if defined(PX4_SPIDEV_LPS22H) + +#include /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) -#define HMC_MAX_SEND_LEN 4 -#define HMC_MAX_RCV_LEN 8 - device::Device *LPS25H_SPI_interface(int bus); class LPS25H_SPI : public device::SPI { public: LPS25H_SPI(int bus, uint32_t device); - virtual ~LPS25H_SPI() = default; + ~LPS25H_SPI() override = default; - virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); - - virtual int ioctl(unsigned operation, unsigned &arg); + int init() override; + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; }; -device::Device * -LPS25H_SPI_interface(int bus) +device::Device *LPS25H_SPI_interface(int bus) { - return new LPS25H_SPI(bus, PX4_SPIDEV_HMC); + return new LPS25H_SPI(bus, PX4_SPIDEV_LPS22H); } LPS25H_SPI::LPS25H_SPI(int bus, uint32_t device) : - SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) + SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LPS25H; + set_device_type(DRV_BARO_DEVTYPE_LPS25H); } -int -LPS25H_SPI::init() +int LPS25H_SPI::init() { - int ret; - - ret = SPI::init(); + int ret = SPI::init(); if (ret != OK) { DEVICE_DEBUG("SPI init failed"); @@ -104,26 +97,7 @@ LPS25H_SPI::init() return OK; } -int -LPS25H_SPI::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: { - ret = -EINVAL; - } - } - - return ret; -} - -int -LPS25H_SPI::write(unsigned address, void *data, unsigned count) +int LPS25H_SPI::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -137,8 +111,7 @@ LPS25H_SPI::write(unsigned address, void *data, unsigned count) return transfer(&buf[0], &buf[0], count + 1); } -int -LPS25H_SPI::read(unsigned address, void *data, unsigned count) +int LPS25H_SPI::read(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -153,4 +126,4 @@ LPS25H_SPI::read(unsigned address, void *data, unsigned count) return ret; } -#endif /* PX4_SPIDEV_HMC */ +#endif /* PX4_SPIDEV_LPS22H */