mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
lps22hb: move to PX4Barometer and cleanup
This commit is contained in:
Daniel Agar
parent
7c7ee115e3
commit
f51715c7ab
@ -38,4 +38,7 @@ px4_add_module(
|
||||
LPS22HB.cpp
|
||||
LPS22HB_I2C.cpp
|
||||
LPS22HB_SPI.cpp
|
||||
DEPENDS
|
||||
drivers_barometer
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
@ -43,20 +43,16 @@
|
||||
#define LPS22HB_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */
|
||||
|
||||
LPS22HB::LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface) :
|
||||
CDev(nullptr),
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
|
||||
_px4_baro(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"))
|
||||
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
|
||||
{
|
||||
}
|
||||
|
||||
LPS22HB::~LPS22HB()
|
||||
{
|
||||
if (_class_instance != -1) {
|
||||
unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance);
|
||||
}
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
@ -64,102 +60,18 @@ LPS22HB::~LPS22HB()
|
||||
delete _interface;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::init()
|
||||
int LPS22HB::init()
|
||||
{
|
||||
int ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_DEBUG("CDev init failed");
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (reset() != OK) {
|
||||
goto out;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
/* register alternate interfaces if we have to */
|
||||
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
|
||||
|
||||
ret = OK;
|
||||
|
||||
_measure_interval = LPS22HB_CONVERSION_INTERVAL;
|
||||
start();
|
||||
|
||||
out:
|
||||
return ret;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::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 = (LPS22HB_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start) {
|
||||
_measure_interval = (LPS22HB_CONVERSION_INTERVAL);
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_interval == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned interval = (1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (interval < (LPS22HB_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
|
||||
LPS22HB::start()
|
||||
void LPS22HB::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
@ -168,18 +80,12 @@ LPS22HB::start()
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::reset()
|
||||
int LPS22HB::reset()
|
||||
{
|
||||
int ret = PX4_ERROR;
|
||||
|
||||
ret = write_reg(CTRL_REG2, BOOT | SWRESET);
|
||||
|
||||
return ret;
|
||||
return write_reg(CTRL_REG2, BOOT | SWRESET);
|
||||
}
|
||||
|
||||
void
|
||||
LPS22HB::RunImpl()
|
||||
void LPS22HB::RunImpl()
|
||||
{
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
@ -194,17 +100,6 @@ LPS22HB::RunImpl()
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_interval > LPS22HB_CONVERSION_INTERVAL) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
ScheduleDelayed(_measure_interval - LPS22HB_CONVERSION_INTERVAL);
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
@ -219,8 +114,7 @@ LPS22HB::RunImpl()
|
||||
ScheduleDelayed(LPS22HB_CONVERSION_INTERVAL);
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::measure()
|
||||
int LPS22HB::measure()
|
||||
{
|
||||
// Send the command to begin a 16-bit measurement.
|
||||
int ret = write_reg(CTRL_REG2, IF_ADD_INC | ONE_SHOT);
|
||||
@ -232,14 +126,11 @@ LPS22HB::measure()
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::collect()
|
||||
int LPS22HB::collect()
|
||||
{
|
||||
perf_begin(_sample_perf);
|
||||
sensor_baro_s new_report;
|
||||
|
||||
/* get measurements from the device : MSB enables register address auto-increment */
|
||||
#pragma pack(push, 1)
|
||||
struct {
|
||||
uint8_t STATUS;
|
||||
uint8_t PRESS_OUT_XL;
|
||||
@ -247,11 +138,10 @@ LPS22HB::collect()
|
||||
uint8_t PRESS_OUT_H;
|
||||
uint8_t TEMP_OUT_L;
|
||||
uint8_t TEMP_OUT_H;
|
||||
} report;
|
||||
#pragma pack(pop)
|
||||
} report{};
|
||||
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
new_report.timestamp = hrt_absolute_time();
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
int ret = _interface->read(STATUS, (uint8_t *)&report, sizeof(report));
|
||||
|
||||
if (ret != OK) {
|
||||
@ -260,50 +150,28 @@ LPS22HB::collect()
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8);
|
||||
float temperature = 42.5f + (TEMP_OUT / 480.0f);
|
||||
_px4_baro.set_temperature(temperature);
|
||||
|
||||
// To obtain the pressure in hPa, take the two’s complement of the complete word and then divide by 4096 LSB/hPa.
|
||||
uint32_t P = report.PRESS_OUT_XL + (report.PRESS_OUT_L << 8) + (report.PRESS_OUT_H << 16);
|
||||
|
||||
uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8);
|
||||
|
||||
/* Pressure and MSL in mBar */
|
||||
new_report.pressure = P / 4096.0f;
|
||||
new_report.temperature = 42.5f + (TEMP_OUT / 480.0f);
|
||||
|
||||
/* get device ID */
|
||||
new_report.device_id = _interface->get_device_id();
|
||||
new_report.error_count = perf_event_count(_comms_errors);
|
||||
|
||||
if (_baro_topic != nullptr) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report);
|
||||
|
||||
} else {
|
||||
bool sensor_is_onboard = !_interface->external();
|
||||
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report, &_orb_class_instance,
|
||||
(sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
|
||||
|
||||
if (_baro_topic == nullptr) {
|
||||
PX4_ERR("advertise failed");
|
||||
}
|
||||
}
|
||||
|
||||
_last_report = new_report;
|
||||
|
||||
ret = OK;
|
||||
float pressure = P / 4096.0f;
|
||||
_px4_baro.update(timestamp_sample, pressure);
|
||||
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::write_reg(uint8_t reg, uint8_t val)
|
||||
int LPS22HB::write_reg(uint8_t reg, uint8_t val)
|
||||
{
|
||||
uint8_t buf = val;
|
||||
return _interface->write(reg, &buf, 1);
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB::read_reg(uint8_t reg, uint8_t &val)
|
||||
int LPS22HB::read_reg(uint8_t reg, uint8_t &val)
|
||||
{
|
||||
uint8_t buf = val;
|
||||
int ret = _interface->read(reg, &buf, 1);
|
||||
@ -311,14 +179,10 @@ LPS22HB::read_reg(uint8_t reg, uint8_t &val)
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
LPS22HB::print_status()
|
||||
void LPS22HB::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
|
||||
PX4_INFO("poll interval: %u", _measure_interval);
|
||||
|
||||
print_message(_last_report);
|
||||
_px4_baro.print_status();
|
||||
}
|
||||
|
||||
@ -36,17 +36,10 @@
|
||||
#include <cstring>
|
||||
|
||||
#include <drivers/device/Device.hpp>
|
||||
#include <drivers/device/i2c.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 <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include <lib/drivers/barometer/PX4Barometer.hpp>
|
||||
|
||||
static constexpr uint8_t WHO_AM_I = 0x0F;
|
||||
static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1;
|
||||
@ -81,7 +74,7 @@ static constexpr uint8_t TEMP_OUT_H = 0x2C;
|
||||
extern device::Device *LPS22HB_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
|
||||
extern device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
class LPS22HB : public cdev::CDev, public I2CSPIDriver<LPS22HB>
|
||||
class LPS22HB : public I2CSPIDriver<LPS22HB>
|
||||
{
|
||||
public:
|
||||
LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface);
|
||||
@ -91,32 +84,21 @@ public:
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
int init();
|
||||
|
||||
void print_status();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
protected:
|
||||
device::Device *_interface;
|
||||
|
||||
private:
|
||||
unsigned _measure_interval{0};
|
||||
PX4Barometer _px4_baro;
|
||||
device::Device *_interface;
|
||||
|
||||
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.
|
||||
*
|
||||
@ -130,7 +112,6 @@ private:
|
||||
*/
|
||||
int reset();
|
||||
|
||||
|
||||
/**
|
||||
* Write a register.
|
||||
*
|
||||
@ -160,8 +141,4 @@ private:
|
||||
* Collect the result of the most recent measurement.
|
||||
*/
|
||||
int collect();
|
||||
|
||||
/* this class has pointer data members, do not allow copying it */
|
||||
LPS22HB(const LPS22HB &);
|
||||
LPS22HB operator=(const LPS22HB &);
|
||||
};
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
|
||||
#include "LPS22HB.hpp"
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#define LPS22HB_ADDRESS 0x5D
|
||||
|
||||
device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency);
|
||||
@ -47,13 +49,13 @@ class LPS22HB_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
LPS22HB_I2C(int bus, int bus_frequency);
|
||||
virtual ~LPS22HB_I2C() = default;
|
||||
~LPS22HB_I2C() override = default;
|
||||
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
int probe() override;
|
||||
|
||||
};
|
||||
|
||||
@ -68,10 +70,9 @@ LPS22HB_I2C::LPS22HB_I2C(int bus, int bus_frequency) :
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB_I2C::probe()
|
||||
int LPS22HB_I2C::probe()
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t id = 0;
|
||||
|
||||
_retries = 10;
|
||||
|
||||
@ -87,11 +88,10 @@ LPS22HB_I2C::probe()
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB_I2C::write(unsigned address, void *data, unsigned count)
|
||||
int LPS22HB_I2C::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
|
||||
#include "LPS22HB.hpp"
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
@ -49,12 +51,13 @@ class LPS22HB_SPI : public device::SPI
|
||||
{
|
||||
public:
|
||||
LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
|
||||
virtual ~LPS22HB_SPI() = default;
|
||||
~LPS22HB_SPI() override = default;
|
||||
|
||||
virtual int init();
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
int read(unsigned address, void *data, unsigned count) override;
|
||||
int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
int probe() override;
|
||||
};
|
||||
|
||||
device::Device *
|
||||
@ -63,21 +66,13 @@ LPS22HB_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi
|
||||
return new LPS22HB_SPI(bus, devid, bus_frequency, spi_mode);
|
||||
}
|
||||
|
||||
LPS22HB_SPI::LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
|
||||
: SPI(DRV_BARO_DEVTYPE_LPS22HB, MODULE_NAME, bus, device, spi_mode, bus_frequency)
|
||||
LPS22HB_SPI::LPS22HB_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
|
||||
SPI(DRV_BARO_DEVTYPE_LPS22HB, MODULE_NAME, bus, device, spi_mode, bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB_SPI::init()
|
||||
int LPS22HB_SPI::probe()
|
||||
{
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
// read WHO_AM_I value
|
||||
uint8_t id = 0;
|
||||
|
||||
@ -91,11 +86,10 @@ LPS22HB_SPI::init()
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB_SPI::write(unsigned address, void *data, unsigned count)
|
||||
int LPS22HB_SPI::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
@ -109,8 +103,7 @@ LPS22HB_SPI::write(unsigned address, void *data, unsigned count)
|
||||
return transfer(&buf[0], &buf[0], count + 1);
|
||||
}
|
||||
|
||||
int
|
||||
LPS22HB_SPI::read(unsigned address, void *data, unsigned count)
|
||||
int LPS22HB_SPI::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
|
||||
@ -34,11 +34,7 @@
|
||||
#include "LPS22HB.hpp"
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
void
|
||||
LPS22HB::print_usage()
|
||||
void LPS22HB::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lps22hb", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("baro");
|
||||
@ -85,7 +81,7 @@ I2CSPIDriverBase *LPS22HB::instantiate(const BusCLIArguments &cli, const BusInst
|
||||
return dev;
|
||||
}
|
||||
|
||||
extern "C" int lps22hb_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int lps22hb_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = LPS22HB;
|
||||
BusCLIArguments cli{true, true};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user