dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

refactor lps25h: use driver base class

This commit is contained in:
Beat Küng 2020-02-26 19:59:48 +01:00 committed by Daniel Agar
parent dc5ffb43a3
commit 081ab729aa
7 changed files with 82 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/bitcraze/crazyflie/init/rc.board_sensors
View File

@ -9,7 +9,7 @@ adc start
mpu9250 -R 12 start
# I2C bypass of mpu
lps25h start
lps25h -I start
# Optical flow deck
vl53lxx start

18
src/drivers/barometer/lps25h/LPS25H.cpp
View File

@ -33,8 +33,8 @@
#include "LPS25H.hpp"
LPS25H::LPS25H(device::Device *interface) :
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
LPS25H::LPS25H(I2CSPIBusOption bus_option, int bus, device::Device *interface) :
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
_px4_barometer(interface->get_device_id()),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@ -46,8 +46,6 @@ LPS25H::LPS25H(device::Device *interface) :
LPS25H::~LPS25H()
{
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
@ -74,11 +72,6 @@ void LPS25H::start()
ScheduleNow();
}
void LPS25H::stop()
{
ScheduleClear();
}
int LPS25H::reset()
{
// Power on
@ -96,7 +89,7 @@ int LPS25H::reset()
return ret;
}
void LPS25H::Run()
void LPS25H::RunImpl()
{
/* collection phase? */
if (_collect_phase) {
@ -115,7 +108,7 @@ void LPS25H::Run()
/*
* Is there a collect->measure gap?
*/
if (_measure_interval > (LPS25H_CONVERSION_INTERVAL)) {
if (_measure_interval > LPS25H_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(_measure_interval - LPS25H_CONVERSION_INTERVAL);
@ -203,8 +196,9 @@ int LPS25H::read_reg(uint8_t reg, uint8_t &val)
return ret;
}
void LPS25H::print_info()
void LPS25H::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);

16
src/drivers/barometer/lps25h/LPS25H.hpp
View File

@ -45,6 +45,7 @@
#include <lib/perf/perf_counter.h>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
/*
* LPS25H internal constants and data structures.
@ -151,21 +152,24 @@
#define FIFO_STATUS_FULL (1 << 6)
#define FIFO_STATUS_WTM (1 << 7)
class LPS25H : public px4::ScheduledWorkItem
class LPS25H : public I2CSPIDriver<LPS25H>
{
public:
LPS25H(device::Device *interface);
LPS25H(I2CSPIBusOption bus_option, int bus, device::Device *interface);
~LPS25H() override;
int init();
void print_info();
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();
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);

7
src/drivers/barometer/lps25h/lps25h.h
View File

@ -44,8 +44,6 @@
#include <drivers/device/Device.hpp>
#include <drivers/device/i2c.h>
#include <drivers/device/spi.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#define ADDR_WHO_AM_I 0x0F
@ -53,6 +51,5 @@
#define ID_WHO_AM_I 0xBD
/* interface factories */
extern device::Device *LPS25H_SPI_interface(int bus);
extern device::Device *LPS25H_I2C_interface(int bus);
typedef device::Device *(*LPS25H_constructor)(int);
extern device::Device *LPS25H_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
extern device::Device *LPS25H_I2C_interface(int bus, int bus_frequency);

12
src/drivers/barometer/lps25h/lps25h_i2c.cpp
View File

@ -43,12 +43,12 @@
#define LPS25H_ADDRESS 0x5D
device::Device *LPS25H_I2C_interface(int bus);
device::Device *LPS25H_I2C_interface(int bus, int bus_frequency);
class LPS25H_I2C : public device::I2C
{
public:
LPS25H_I2C(int bus);
LPS25H_I2C(int bus, int bus_frequency);
virtual ~LPS25H_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
@ -59,13 +59,13 @@ protected:
};
device::Device *LPS25H_I2C_interface(int bus)
device::Device *LPS25H_I2C_interface(int bus, int bus_frequency)
{
return new LPS25H_I2C(bus);
return new LPS25H_I2C(bus, bus_frequency);
}
LPS25H_I2C::LPS25H_I2C(int bus) :
I2C("LPS25H_I2C", nullptr, bus, LPS25H_ADDRESS, 400000)
LPS25H_I2C::LPS25H_I2C(int bus, int bus_frequency) :
I2C("LPS25H_I2C", nullptr, bus, LPS25H_ADDRESS, bus_frequency)
{
set_device_type(DRV_BARO_DEVTYPE_LPS25H);
}

211
src/drivers/barometer/lps25h/lps25h_main.cpp
View File

@ -33,195 +33,84 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include "LPS25H.hpp"
enum class LPS25H_BUS {
ALL = 0,
I2C_INTERNAL,
I2C_EXTERNAL,
SPI_INTERNAL,
SPI_EXTERNAL
};
namespace lps25h
void
LPS25H::print_usage()
{
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;
PRINT_MODULE_USAGE_NAME("lps25h", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("baro");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
static bool start_bus(lps25h_bus_option &bus)
I2CSPIDriverBase *LPS25H::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = bus.interface_constructor(bus.busnum);
device::Device *interface = nullptr;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = LPS25H_I2C_interface(iterator.bus(), cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = LPS25H_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) {
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
delete interface;
return false;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
LPS25H *dev = new LPS25H(interface);
LPS25H *dev = new LPS25H(iterator.configuredBusOption(), iterator.bus(), 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;
return nullptr;
}
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;
}
if (OK != dev->init()) {
delete dev;
return nullptr;
}
return PX4_ERROR;
return dev;
}
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;
using ThisDriver = LPS25H;
BusCLIArguments cli{true, true};
cli.default_spi_frequency = 11 * 1000 * 1000;
const char *verb = cli.parseDefaultArguments(argc, argv);
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 (!verb) {
ThisDriver::print_usage();
return -1;
}
if (myoptind >= argc) {
return lps25h::usage();
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_LPS25H);
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 ThisDriver::module_start(cli, iterator);
}
return lps25h::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

18
src/drivers/barometer/lps25h/lps25h_spi.cpp
View File

@ -39,20 +39,18 @@
#include "lps25h.h"
#if defined(PX4_SPIDEV_LPS22H)
#include <drivers/device/spi.h>
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
device::Device *LPS25H_SPI_interface(int bus);
device::Device *LPS25H_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
class LPS25H_SPI : public device::SPI
{
public:
LPS25H_SPI(int bus, uint32_t device);
LPS25H_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
~LPS25H_SPI() override = default;
int init() override;
@ -61,13 +59,13 @@ public:
};
device::Device *LPS25H_SPI_interface(int bus)
device::Device *LPS25H_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
{
return new LPS25H_SPI(bus, PX4_SPIDEV_LPS22H);
return new LPS25H_SPI(bus, devid, bus_frequency, spi_mode);
}
LPS25H_SPI::LPS25H_SPI(int bus, uint32_t device) :
SPI("LPS25H_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000)
LPS25H_SPI::LPS25H_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
SPI("LPS25H_SPI", nullptr, bus, device, spi_mode, bus_frequency)
{
set_device_type(DRV_BARO_DEVTYPE_LPS25H);
}
@ -84,7 +82,7 @@ int LPS25H_SPI::init()
// read WHO_AM_I value
uint8_t id;
if (read(ADDR_ID, &id, 1)) {
if (read(ADDR_WHO_AM_I, &id, 1)) {
DEVICE_DEBUG("read_reg fail");
return -EIO;
}
@ -125,5 +123,3 @@ int LPS25H_SPI::read(unsigned address, void *data, unsigned count)
memcpy(data, &buf[1], count);
return ret;
}
#endif /* PX4_SPIDEV_LPS22H */