From 6b2daef5ec79bb678d9960966258f1900eae0d1e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 27 Apr 2018 13:10:42 -0400 Subject: [PATCH] lps22hb barometer driver --- src/drivers/barometer/CMakeLists.txt | 1 + src/drivers/barometer/lps22hb/CMakeLists.txt | 42 ++ src/drivers/barometer/lps22hb/LPS22HB.cpp | 375 ++++++++++++++++++ src/drivers/barometer/lps22hb/LPS22HB.hpp | 195 +++++++++ src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp | 122 ++++++ src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp | 137 +++++++ .../barometer/lps22hb/lps22hb_main.cpp | 288 ++++++++++++++ src/drivers/barometer/lps25h/lps25h_spi.cpp | 3 +- src/drivers/drv_sensor.h | 1 + 9 files changed, 1162 insertions(+), 2 deletions(-) create mode 100644 src/drivers/barometer/lps22hb/CMakeLists.txt create mode 100644 src/drivers/barometer/lps22hb/LPS22HB.cpp create mode 100644 src/drivers/barometer/lps22hb/LPS22HB.hpp create mode 100644 src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp create mode 100644 src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp create mode 100644 src/drivers/barometer/lps22hb/lps22hb_main.cpp diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index 0b07985816..8e61b98696 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(bmp280) +add_subdirectory(lps22hb) #add_subdirectory(lps25h) # not ready for general inclusion #add_subdirectory(mpl3115a2) # not ready for general inclusion add_subdirectory(ms5611) diff --git a/src/drivers/barometer/lps22hb/CMakeLists.txt b/src/drivers/barometer/lps22hb/CMakeLists.txt new file mode 100644 index 0000000000..de7ade623e --- /dev/null +++ b/src/drivers/barometer/lps22hb/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__barometer__lps22hb + MAIN lps22hb + COMPILE_FLAGS + SRCS + lps22hb_main.cpp + LPS22HB.cpp + LPS22HB_I2C.cpp + LPS22HB_SPI.cpp + ) diff --git a/src/drivers/barometer/lps22hb/LPS22HB.cpp b/src/drivers/barometer/lps22hb/LPS22HB.cpp new file mode 100644 index 0000000000..7d5261c75e --- /dev/null +++ b/src/drivers/barometer/lps22hb/LPS22HB.cpp @@ -0,0 +1,375 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file lps22hb.cpp + * + * Driver for the LPS22HB barometer connected via I2C or SPI. + */ + +#include "LPS22HB.hpp" + +#include + +/* Max measurement rate is 25Hz */ +#define LPS22HB_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */ + +LPS22HB::LPS22HB(device::Device *interface, const char *path) : + CDev("LPS22HB", path), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, "lps22hb_read")), + _comms_errors(perf_alloc(PC_COUNT, "lps22hb_comms_errors")) +{ + // 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_LPS22HB; +} + +LPS22HB::~LPS22HB() +{ + /* make sure we are truly inactive */ + stop(); + + if (_class_instance != -1) { + unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + + delete _interface; +} + +int +LPS22HB::init() +{ + int ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + 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; + + PX4_INFO("starting"); + _measure_ticks = USEC2TICK(LPS22HB_CONVERSION_INTERVAL); + start(); + +out: + return ret; +} + +int +LPS22HB::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + unsigned dummy = arg; + + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) 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(LPS22HB_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + _measure_ticks = USEC2TICK(LPS22HB_CONVERSION_INTERVAL); + start(); + } + + 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(LPS22HB_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(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCRESET: + return reset(); + + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +void +LPS22HB::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LPS22HB::cycle_trampoline, this, 1); +} + +void +LPS22HB::stop() +{ + work_cancel(HPWORK, &_work); +} + +int +LPS22HB::reset() +{ + int ret = PX4_ERROR; + + ret = write_reg(CTRL_REG2, BOOT | I2C_DIS | SWRESET); + + return ret; +} + +void +LPS22HB::cycle_trampoline(void *arg) +{ + LPS22HB *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +LPS22HB::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + DEVICE_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_ticks > USEC2TICK(LPS22HB_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LPS22HB::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LPS22HB_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + DEVICE_DEBUG("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LPS22HB::cycle_trampoline, + this, + USEC2TICK(LPS22HB_CONVERSION_INTERVAL)); +} + +int +LPS22HB::measure() +{ + // Send the command to begin a 16-bit measurement. + int ret = write_reg(CTRL_REG2, IF_ADD_INC | ONE_SHOT); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int +LPS22HB::collect() +{ + perf_begin(_sample_perf); + struct baro_report new_report; + + /* get measurements from the device : MSB enables register address auto-increment */ +#pragma pack(push, 1) + struct { + uint8_t STATUS; + uint8_t PRESS_OUT_XL; + uint8_t PRESS_OUT_L; + uint8_t PRESS_OUT_H; + uint8_t TEMP_OUT_L; + uint8_t TEMP_OUT_H; + } report; +#pragma pack(pop) + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + new_report.timestamp = hrt_absolute_time(); + int ret = _interface->read(STATUS, (uint8_t *)&report, sizeof(report)); + + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + // To obtain the pressure in hPa, take the two’s complement of the complete word and then divide by 4096 LSB/hPa. + uint32_t P = report.PRESS_OUT_XL + (report.PRESS_OUT_L << 8) + (report.PRESS_OUT_H << 16); + + uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8); + + /* Pressure and MSL in mBar */ + new_report.pressure = P / 4096.0f; + new_report.temperature = 42.5f + (TEMP_OUT / 480.0f); + + /* get device ID */ + new_report.device_id = _device_id.devid; + new_report.error_count = perf_event_count(_comms_errors); + + if (!(_pub_blocked)) { + + if (_baro_topic != nullptr) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report); + + } else { + bool sensor_is_onboard = !_interface->external(); + _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) { + DEVICE_DEBUG("ADVERT FAIL"); + } + } + } + + _last_report = new_report; + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +int +LPS22HB::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf = val; + return _interface->write(reg, &buf, 1); +} + +int +LPS22HB::read_reg(uint8_t reg, uint8_t &val) +{ + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; +} + +void +LPS22HB::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + PX4_INFO("poll interval: %u ticks", _measure_ticks); + + print_message(_last_report); +} diff --git a/src/drivers/barometer/lps22hb/LPS22HB.hpp b/src/drivers/barometer/lps22hb/LPS22HB.hpp new file mode 100644 index 0000000000..9d66e35490 --- /dev/null +++ b/src/drivers/barometer/lps22hb/LPS22HB.hpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include + +static constexpr uint8_t WHO_AM_I = 0x0F; +static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1; + +static constexpr uint8_t CTRL_REG1 = 0x10; +static constexpr uint8_t CTRL_REG2 = 0x11; +static constexpr uint8_t CTRL_REG3 = 0x12; + +#define BOOT (1 << 7) +#define FIFO_EN (1 << 6) +#define STOP_ON_FTH (1 << 5) +#define IF_ADD_INC (1 << 4) +#define I2C_DIS (1 << 3) +#define SWRESET (1 << 2) +#define ONE_SHOT (1 << 0) + +static constexpr uint8_t STATUS = 0x27; + +#define T_OR (1 << 5) // Temperature data overrun. +#define P_OR (1 << 4) // Pressure data overrun. +#define T_DA (1 << 1) // Temperature data available. +#define P_DA (1 << 0) // Pressure data available. + +static constexpr uint8_t PRESS_OUT_XL = 0x28; +static constexpr uint8_t PRESS_OUT_L = 0x29; +static constexpr uint8_t PRESS_OUT_H = 0x2A; + +static constexpr uint8_t TEMP_OUT_L = 0x2B; +static constexpr uint8_t TEMP_OUT_H = 0x2C; + +/* interface factories */ +extern device::Device *LPS22HB_SPI_interface(int bus); +extern device::Device *LPS22HB_I2C_interface(int bus); +typedef device::Device *(*LPS22HB_constructor)(int); + +class LPS22HB : public device::CDev +{ +public: + LPS22HB(device::Device *interface, const char *path); + virtual ~LPS22HB(); + + virtual int init(); + + 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; + +private: + work_s _work{}; + unsigned _measure_ticks{0}; + + 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; + + baro_report _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 cycle(); + + /** + * 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); + + /** + * 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 */ + LPS22HB(const LPS22HB &); + LPS22HB operator=(const LPS22HB &); +}; diff --git a/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp new file mode 100644 index 0000000000..389b81f323 --- /dev/null +++ b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file lps22hb_i2c.cpp + * + * I2C interface for lps22hb + */ + +#include "LPS22HB.hpp" + +#include + +#include + +#include +#include + +#include "board_config.h" + +#define LPS22HB_ADDRESS 0x5D + +device::Device *LPS22HB_I2C_interface(int bus); + +class LPS22HB_I2C : public device::I2C +{ +public: + LPS22HB_I2C(int bus); + virtual ~LPS22HB_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +protected: + virtual int probe(); + +}; + +device::Device * +LPS22HB_I2C_interface(int bus) +{ + return new LPS22HB_I2C(bus); +} + +LPS22HB_I2C::LPS22HB_I2C(int bus) : + I2C("LPS22HB_I2C", nullptr, bus, LPS22HB_ADDRESS, 400000) +{ +} + +int +LPS22HB_I2C::probe() +{ + uint8_t id; + + _retries = 10; + + if (read(WHO_AM_I, &id, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if (id != LPS22HB_ID_WHO_AM_I) { + DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", LPS22HB_ID_WHO_AM_I, id); + return -EIO; + } + + return OK; +} + +int +LPS22HB_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +LPS22HB_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} diff --git a/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp b/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp new file mode 100644 index 0000000000..8f9187ee0a --- /dev/null +++ b/src/drivers/barometer/lps22hb/LPS22HB_SPI.cpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LPS22HB_SPI.cpp + * + * SPI interface for LPS22HB + */ + +#include "LPS22HB.hpp" + +#include + +#include + +#include +#include + +#ifdef PX4_SPIDEV_LPS22HB + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +device::Device *LPS22HB_SPI_interface(int bus); + +class LPS22HB_SPI : public device::SPI +{ +public: + LPS22HB_SPI(int bus, uint32_t device); + virtual ~LPS22HB_SPI() = default; + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +}; + +device::Device * +LPS22HB_SPI_interface(int bus) +{ + return new LPS22HB_SPI(bus, PX4_SPIDEV_LPS22HB); +} + +LPS22HB_SPI::LPS22HB_SPI(int bus, uint32_t device) : SPI("LPS22HB_SPI", nullptr, bus, device, SPIDEV_MODE3, 10000000) +{ + _device_id.devid_s.devtype = DRV_BARO_DEVTYPE_LPS22HB; +} + +int +LPS22HB_SPI::init() +{ + int ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t id = 0; + + if (read(WHO_AM_I, &id, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + if (id != LPS22HB_ID_WHO_AM_I) { + DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", LPS22HB_ID_WHO_AM_I, id); + return -EIO; + } + + return OK; +} + +int +LPS22HB_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +LPS22HB_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_LPS22HB */ diff --git a/src/drivers/barometer/lps22hb/lps22hb_main.cpp b/src/drivers/barometer/lps22hb/lps22hb_main.cpp new file mode 100644 index 0000000000..1e62596076 --- /dev/null +++ b/src/drivers/barometer/lps22hb/lps22hb_main.cpp @@ -0,0 +1,288 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "LPS22HB.hpp" + +#include + +extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]); + +enum LPS22HB_BUS { + LPS22HB_BUS_ALL = 0, + LPS22HB_BUS_I2C_INTERNAL, + LPS22HB_BUS_I2C_EXTERNAL, + LPS22HB_BUS_SPI +}; + +/** + * Local functions in support of the shell command. + */ +namespace lps22hb +{ + +struct lps22hb_bus_option { + enum LPS22HB_BUS busid; + const char *devpath; + LPS22HB_constructor interface_constructor; + uint8_t busnum; + LPS22HB *dev; +} bus_options[] = { + { LPS22HB_BUS_I2C_EXTERNAL, "/dev/lps22hb_ext", &LPS22HB_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_ONBOARD + { LPS22HB_BUS_I2C_INTERNAL, "/dev/lps22hb_int", &LPS22HB_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_LPS22HB + { LPS22HB_BUS_SPI, "/dev/lps22hb_spi", &LPS22HB_SPI_interface, PX4_SPI_BUS_SENSOR4, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +int start(enum LPS22HB_BUS busid); +bool start_bus(struct lps22hb_bus_option &bus); +struct lps22hb_bus_option &find_bus(enum LPS22HB_BUS busid); +int reset(enum LPS22HB_BUS busid); +int info(); +void usage(); + +/** + * start driver for a specific bus option + */ +bool +start_bus(struct lps22hb_bus_option &bus) +{ + PX4_INFO("starting %s", bus.devpath); + + if (bus.dev != nullptr) { + PX4_WARN("bus option already started"); + return false; + } + + device::Device *interface = bus.interface_constructor(bus.busnum); + + if (interface->init() != OK) { + delete interface; + PX4_WARN("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new LPS22HB(interface, bus.devpath); + + if (bus.dev != nullptr && OK != bus.dev->init()) { + PX4_WARN("init failed"); + delete bus.dev; + bus.dev = nullptr; + 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 (px4_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. + */ +int +start(enum LPS22HB_BUS busid) +{ + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == LPS22HB_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != LPS22HB_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i]); + } + + if (!started) { + return PX4_ERROR; + } + + return PX4_OK; +} + +/** + * find a bus structure for a busid + */ +struct lps22hb_bus_option &find_bus(enum LPS22HB_BUS busid) +{ + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == LPS22HB_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)busid); +} + +/** + * Reset the driver. + */ +int +reset(enum LPS22HB_BUS busid) +{ + struct lps22hb_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed"); + return PX4_ERROR; + } + + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return PX4_ERROR; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return PX4_ERROR; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + struct lps22hb_bus_option &bus = bus_options[i]; + + if (bus.dev != nullptr) { + warnx("%s", bus.devpath); + bus.dev->print_info(); + } + } + + return 0; +} + +void +usage() +{ + PX4_INFO("missing command: try 'start', 'info', 'reset'"); + PX4_INFO("options:"); + PX4_INFO(" -X (external I2C bus)"); + PX4_INFO(" -I (internal I2C bus)"); + PX4_INFO(" -S (external SPI bus)"); + PX4_INFO(" -s (internal SPI bus)"); +} + +} // namespace + +int +lps22hb_main(int argc, char *argv[]) +{ + enum LPS22HB_BUS busid = LPS22HB_BUS_ALL; + int ch; + + while ((ch = getopt(argc, argv, "XIS:")) != EOF) { + switch (ch) { +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + + case 'I': + busid = LPS22HB_BUS_I2C_INTERNAL; + break; +#endif + + case 'X': + busid = LPS22HB_BUS_I2C_EXTERNAL; + break; + + case 'S': + busid = LPS22HB_BUS_SPI; + break; + + default: + lps22hb::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + return lps22hb::start(busid); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + return lps22hb::reset(busid); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + return lps22hb::info(); + } + + PX4_WARN("unrecognised command, try 'start', 'reset' or 'info'"); + return 0; +} diff --git a/src/drivers/barometer/lps25h/lps25h_spi.cpp b/src/drivers/barometer/lps25h/lps25h_spi.cpp index aad95f44da..f72458133c 100644 --- a/src/drivers/barometer/lps25h/lps25h_spi.cpp +++ b/src/drivers/barometer/lps25h/lps25h_spi.cpp @@ -63,7 +63,6 @@ /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) #define HMC_MAX_SEND_LEN 4 #define HMC_MAX_RCV_LEN 8 @@ -170,7 +169,7 @@ LPS25H_SPI::read(unsigned address, void *data, unsigned count) return -EIO; } - buf[0] = address | DIR_READ | ADDR_INCREMENT; + buf[0] = address | DIR_READ; int ret = transfer(&buf[0], &buf[0], count + 1); memcpy(data, &buf[1], count); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 64e265ed5b..9fff0f8a95 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -103,6 +103,7 @@ #define DRV_ACC_DEVTYPE_ADIS16448 0x55 #define DRV_MAG_DEVTYPE_ADIS16448 0x56 #define DRV_GYR_DEVTYPE_ADIS16448 0x57 +#define DRV_BARO_DEVTYPE_LPS22HB 0x58 /* * ioctl() definitions