diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index 5c466e7cee..eaa73ef982 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -36,5 +36,6 @@ add_subdirectory(bmp388) add_subdirectory(dps310) add_subdirectory(lps22hb) #add_subdirectory(lps25h) # not ready for general inclusion +add_subdirectory(lps33hw) #add_subdirectory(mpl3115a2) # not ready for general inclusion add_subdirectory(ms5611) diff --git a/src/drivers/barometer/lps33hw/CMakeLists.txt b/src/drivers/barometer/lps33hw/CMakeLists.txt new file mode 100644 index 0000000000..6e8ce1c9ea --- /dev/null +++ b/src/drivers/barometer/lps33hw/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ +px4_add_module( + MODULE drivers__barometer__lps33hw + MAIN lps33hw + SRCS + lps33hw.cpp + lps33hw.hpp + lps33hw_i2c.cpp + lps33hw_spi.cpp + lps33hw_main.cpp + DEPENDS + drivers_barometer + px4_work_queue + ) diff --git a/src/drivers/barometer/lps33hw/lps33hw.cpp b/src/drivers/barometer/lps33hw/lps33hw.cpp new file mode 100644 index 0000000000..0f9be5895d --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 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 "lps33hw.hpp" + +using namespace ST_LPS33HW; + +namespace lps33hw +{ + +template +static void getTwosComplement(T &raw, uint8_t length) +{ + if (raw & ((T)1 << (length - 1))) { + raw -= (T)1 << length; + } +} + +LPS33HW::LPS33HW(I2CSPIBusOption bus_option, int bus, device::Device *interface) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, + interface->get_device_address()), + _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": comm errors")) +{ +} + +LPS33HW::~LPS33HW() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); + + delete _interface; +} + +int +LPS33HW::init() +{ + uint8_t who_am_i; + int ret = RegisterRead(Register::WHO_AM_I, who_am_i); + + if (ret != 0) { + PX4_DEBUG("read failed (%i)", ret); + return ret; + } + + if (who_am_i != WHO_AM_I_VALUE) { + PX4_DEBUG("WHO_AM_I mismatch (%i)", who_am_i); + return PX4_ERROR; + } + + ret = reset(); + + if (ret != OK) { + PX4_DEBUG("reset failed"); + return ret; + } + + start(); + + return PX4_OK; +} + +int +LPS33HW::reset() +{ + // Soft Reset + int ret = RegisterWrite(Register::CTRL_REG2, SWRESET); + + // wait until SWRESET goes back to 0 + int i; + + for (i = 0; i < 10; ++i) { + uint8_t val; + ret = RegisterRead(Register::CTRL_REG2, val); + + if (ret == 0 && (val & SWRESET) == 0) { + break; + } + + usleep(1000); + } + + if (i >= 10) { + PX4_ERR("reset failed"); + return PX4_ERROR; + } + + // Configure sampling rate + ret = RegisterWrite(Register::CTRL_REG1, ODR_75HZ | BDU); + + return ret; +} + +void +LPS33HW::start() +{ + ScheduleOnInterval(1000000 / SAMPLE_RATE); +} + +void +LPS33HW::RunImpl() +{ + perf_begin(_sample_perf); + + uint8_t data[6]; + + if (_interface->read((uint8_t)Register::STATUS, data, sizeof(data)) != PX4_OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return; + } + + uint8_t status = data[0]; + + if ((status & P_DA) == 0) { // check if pressure data is available + perf_end(_sample_perf); + return; + } + + hrt_abstime timestamp_sample = hrt_absolute_time(); + float temp = ((int16_t)data[4] | (data[5] << 8)) / 100.f; + + int32_t Praw = (int32_t)data[1] | (data[2] << 8) | (data[3] << 16); + getTwosComplement(Praw, 24); + float pressure_hPa = Praw / 4096.f; + + _px4_barometer.set_error_count(perf_event_count(_comms_errors)); + _px4_barometer.set_temperature(temp); + _px4_barometer.update(timestamp_sample, pressure_hPa); // hPascals -> Millibar + + perf_end(_sample_perf); +} + +int +LPS33HW::RegisterRead(Register reg, uint8_t &val) +{ + return _interface->read((uint8_t)reg, &val, 1); +} + +int +LPS33HW::RegisterWrite(Register reg, uint8_t value) +{ + return _interface->write((uint8_t)reg, &value, 1); +} + +void +LPS33HW::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +} // namespace lps33hw diff --git a/src/drivers/barometer/lps33hw/lps33hw.hpp b/src/drivers/barometer/lps33hw/lps33hw.hpp new file mode 100644 index 0000000000..b84a988df7 --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file lps33hw.hpp + * + * Driver for the Infineon LPS33HW barometer connected via I2C or SPI. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "lps33hw_registers.hpp" + +namespace lps33hw +{ + +using ST_LPS33HW::Register; + +class LPS33HW : public I2CSPIDriver +{ +public: + LPS33HW(I2CSPIBusOption bus_option, int bus, device::Device *interface); + virtual ~LPS33HW(); + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + int init(); + + void print_status(); + void RunImpl(); + +private: + + void start(); + int reset(); + + int RegisterRead(Register reg, uint8_t &val); + int RegisterWrite(Register reg, uint8_t val); + + static constexpr uint32_t SAMPLE_RATE{75}; + + PX4Barometer _px4_barometer; + + device::Device *_interface; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; +}; + +} // namespace lps33hw diff --git a/src/drivers/barometer/lps33hw/lps33hw_i2c.cpp b/src/drivers/barometer/lps33hw/lps33hw_i2c.cpp new file mode 100644 index 0000000000..2aef74a91f --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw_i2c.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file lps33hw_i2c.cpp + * + * I2C interface for LPS33HW + */ + +#include + +namespace lps33hw +{ + +device::Device *LPS33HW_I2C_interface(uint8_t bus, uint32_t address, int bus_frequency); + +class LPS33HW_I2C : public device::I2C +{ +public: + LPS33HW_I2C(uint8_t bus, uint32_t address, int bus_frequency); + virtual ~LPS33HW_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +}; + +device::Device * +LPS33HW_I2C_interface(uint8_t bus, uint32_t address, int bus_frequency) +{ + return new LPS33HW_I2C(bus, address, bus_frequency); +} + +LPS33HW_I2C::LPS33HW_I2C(uint8_t bus, uint32_t address, int bus_frequency) : + I2C(DRV_BARO_DEVTYPE_LPS33HW, MODULE_NAME, bus, address, bus_frequency) +{ +} + +int +LPS33HW_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int +LPS33HW_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); +} + +} // namespace lps33hw diff --git a/src/drivers/barometer/lps33hw/lps33hw_main.cpp b/src/drivers/barometer/lps33hw/lps33hw_main.cpp new file mode 100644 index 0000000000..a599d59f70 --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw_main.cpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 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 "lps33hw.hpp" + +namespace lps33hw +{ +extern device::Device *LPS33HW_SPI_interface(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *LPS33HW_I2C_interface(uint8_t bus, uint32_t device, int bus_frequency); +} + +#include +#include + +using namespace lps33hw; + +void +LPS33HW::print_usage() +{ + PRINT_MODULE_USAGE_NAME("lps33hw", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x5D); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *LPS33HW::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + device::Device *interface = nullptr; + + if (iterator.busType() == BOARD_I2C_BUS) { + interface = LPS33HW_I2C_interface(iterator.bus(), cli.i2c_address, cli.bus_frequency); + + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = LPS33HW_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } + + if (interface == nullptr) { + PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + LPS33HW *dev = new LPS33HW(iterator.configuredBusOption(), iterator.bus(), interface); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +extern "C" int lps33hw_main(int argc, char *argv[]) +{ + using ThisDriver = LPS33HW; + BusCLIArguments cli{true, true}; + cli.i2c_address = 0x5D; + cli.default_i2c_frequency = 400000; + cli.default_spi_frequency = 10 * 1000 * 1000; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_LPS33HW); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/barometer/lps33hw/lps33hw_registers.hpp b/src/drivers/barometer/lps33hw/lps33hw_registers.hpp new file mode 100644 index 0000000000..e60e658012 --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw_registers.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace ST_LPS33HW +{ + +static constexpr uint8_t WHO_AM_I_VALUE = 0b10110001; + +enum class +Register : uint8_t { + + WHO_AM_I = 0x0F, + CTRL_REG1 = 0x10, + CTRL_REG2 = 0x11, + CTRL_REG3 = 0x12, + + REF_P_XL = 0x15, + REF_P_L = 0x16, + REF_P_H = 0x17, + + RPDS_L = 0x18, + RPDS_H = 0x19, + + RES_CONF = 0x1A, + + STATUS = 0x27, + PRESS_OUT_XL = 0x28, + PRESS_OUT_L = 0x29, + PRESS_OUT_H = 0x2A, + TEMP_OUT_L = 0x2B, + TEMP_OUT_H = 0x2C, +}; + +enum CTRL_REG1 : uint8_t { + ODR_75HZ = Bit4 | Bit6, // Continous 75Hz update rate + EN_LPFP = Bit3, // enable low-pass filter + BDU = Bit1, // block data update +}; + +enum CTRL_REG2 : uint8_t { + SWRESET = Bit2, +}; + +enum STATUS : uint8_t { + P_DA = Bit0, // Pressure data available + T_DA = Bit1, // Temp data available +}; + +} // namespace ST_LPS33HW diff --git a/src/drivers/barometer/lps33hw/lps33hw_spi.cpp b/src/drivers/barometer/lps33hw/lps33hw_spi.cpp new file mode 100644 index 0000000000..307d91ce90 --- /dev/null +++ b/src/drivers/barometer/lps33hw/lps33hw_spi.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file lps33hw_spi.cpp + * + * SPI interface for LPS33HW + * + * Note: the driver was not tested with SPI + */ + +#include + +namespace lps33hw +{ + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +device::Device *LPS33HW_SPI_interface(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + +class LPS33HW_SPI : public device::SPI +{ +public: + LPS33HW_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~LPS33HW_SPI() = default; + + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +}; + +device::Device * +LPS33HW_SPI_interface(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) +{ + return new LPS33HW_SPI(bus, device, bus_frequency, spi_mode); +} + +LPS33HW_SPI::LPS33HW_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_LPS33HW, MODULE_NAME, bus, device, spi_mode, bus_frequency) +{ +} + +int +LPS33HW_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; +} + +int +LPS33HW_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); +} + +} // namespace lps33hw diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6caa0cf7f8..fda1e5007d 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -107,6 +107,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x49 #define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4A #define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4B +#define DRV_BARO_DEVTYPE_LPS33HW 0x4C #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52