Daniel Agar
5 years ago
8 changed files with 659 additions and 1053 deletions
@ -0,0 +1,212 @@
@@ -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(); |
||||
} |
||||
@ -0,0 +1,184 @@
@@ -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 <drivers/device/Device.hpp> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <lib/drivers/barometer/PX4Barometer.hpp> |
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||
|
||||
/*
|
||||
* 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; |
||||
}; |
||||
@ -1,968 +0,0 @@
@@ -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<sensor_baro_s *>(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'"); |
||||
} |
||||
@ -0,0 +1,227 @@
@@ -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 <px4_platform_common/px4_config.h> |
||||
#include <px4_platform_common/getopt.h> |
||||
|
||||
#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(); |
||||
} |
||||
Loading…
Reference in new issue