mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 13:02:25 +08:00
lps25h: move to PX4Barometer and cleanup
This commit is contained in:
Daniel Agar
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
@@ -31,12 +31,15 @@
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__lps25h
|
||||
MODULE drivers__barometer__lps25h
|
||||
MAIN lps25h
|
||||
COMPILE_FLAGS
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
SRCS
|
||||
lps25h.cpp
|
||||
LPS25H.cpp
|
||||
LPS25H.hpp
|
||||
lps25h_main.cpp
|
||||
lps25h_i2c.cpp
|
||||
lps25h_spi.cpp
|
||||
DEPENDS
|
||||
drivers_barometer
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
@@ -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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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;
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
@@ -43,16 +43,10 @@
|
||||
|
||||
#include <drivers/device/Device.hpp>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
|
||||
#define ADDR_WHO_AM_I 0x0F
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,6 +39,8 @@
|
||||
|
||||
#include "lps25h.h"
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#define LPS25H_ADDRESS 0x5D
|
||||
|
||||
device::Device *LPS25H_I2C_interface(int bus);
|
||||
@@ -47,20 +49,17 @@ class LPS25H_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
LPS25H_I2C(int bus);
|
||||
virtual ~LPS25H_I2C() = default;
|
||||
virtual ~LPS25H_I2C() override = default;
|
||||
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
|
||||
virtual int ioctl(unsigned operation, unsigned &arg);
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
int probe();
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
LPS25H_I2C_interface(int bus)
|
||||
device::Device *LPS25H_I2C_interface(int bus)
|
||||
{
|
||||
return new LPS25H_I2C(bus);
|
||||
}
|
||||
@@ -68,27 +67,10 @@ LPS25H_I2C_interface(int bus)
|
||||
LPS25H_I2C::LPS25H_I2C(int bus) :
|
||||
I2C("LPS25H_I2C", nullptr, bus, LPS25H_ADDRESS, 400000)
|
||||
{
|
||||
set_device_type(DRV_BARO_DEVTYPE_LPS25H);
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_I2C::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
switch (operation) {
|
||||
|
||||
case DEVIOCGDEVICEID:
|
||||
return CDev::ioctl(nullptr, operation, arg);
|
||||
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_I2C::probe()
|
||||
int LPS25H_I2C::probe()
|
||||
{
|
||||
uint8_t id;
|
||||
|
||||
@@ -109,8 +91,7 @@ LPS25H_I2C::probe()
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_I2C::write(unsigned address, void *data, unsigned count)
|
||||
int LPS25H_I2C::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,49 +39,42 @@
|
||||
|
||||
#include "lps25h.h"
|
||||
|
||||
#ifdef PX4_SPIDEV_HMC
|
||||
#if defined(PX4_SPIDEV_LPS22H)
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
|
||||
#define HMC_MAX_SEND_LEN 4
|
||||
#define HMC_MAX_RCV_LEN 8
|
||||
|
||||
device::Device *LPS25H_SPI_interface(int bus);
|
||||
|
||||
class LPS25H_SPI : public device::SPI
|
||||
{
|
||||
public:
|
||||
LPS25H_SPI(int bus, uint32_t device);
|
||||
virtual ~LPS25H_SPI() = default;
|
||||
~LPS25H_SPI() override = default;
|
||||
|
||||
virtual int init();
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
|
||||
virtual int ioctl(unsigned operation, unsigned &arg);
|
||||
int init() override;
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
LPS25H_SPI_interface(int bus)
|
||||
device::Device *LPS25H_SPI_interface(int bus)
|
||||
{
|
||||
return new LPS25H_SPI(bus, PX4_SPIDEV_HMC);
|
||||
return new LPS25H_SPI(bus, PX4_SPIDEV_LPS22H);
|
||||
}
|
||||
|
||||
LPS25H_SPI::LPS25H_SPI(int bus, uint32_t device) :
|
||||
SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
|
||||
SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LPS25H;
|
||||
set_device_type(DRV_BARO_DEVTYPE_LPS25H);
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_SPI::init()
|
||||
int LPS25H_SPI::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = SPI::init();
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed");
|
||||
@@ -104,26 +97,7 @@ LPS25H_SPI::init()
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_SPI::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
switch (operation) {
|
||||
|
||||
case DEVIOCGDEVICEID:
|
||||
return CDev::ioctl(nullptr, operation, arg);
|
||||
|
||||
default: {
|
||||
ret = -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_SPI::write(unsigned address, void *data, unsigned count)
|
||||
int LPS25H_SPI::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
@@ -137,8 +111,7 @@ LPS25H_SPI::write(unsigned address, void *data, unsigned count)
|
||||
return transfer(&buf[0], &buf[0], count + 1);
|
||||
}
|
||||
|
||||
int
|
||||
LPS25H_SPI::read(unsigned address, void *data, unsigned count)
|
||||
int LPS25H_SPI::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
@@ -153,4 +126,4 @@ LPS25H_SPI::read(unsigned address, void *data, unsigned count)
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif /* PX4_SPIDEV_HMC */
|
||||
#endif /* PX4_SPIDEV_LPS22H */
|
||||
|
||||
Reference in New Issue
Block a user