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

refactor lsm303agr: use driver base class

This commit is contained in:
Beat Küng 2020-03-17 18:08:44 +01:00 committed by Daniel Agar
parent 4d511868e6
commit 74db018012
4 changed files with 70 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/av/x-v1/init/rc.board_sensors
View File

@ -9,7 +9,7 @@ adis16477 -s -R 8 start
lps22hb -s start
lsm303agr -R 4 start
lsm303agr -s -R 4 start
ms4525_airspeed -T 4515 -I -b 3 start

38
src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp
View File

@ -61,9 +61,10 @@ static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40;
*/
#define LSM303AGR_TIMER_REDUCTION 200
LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation) :
SPI("LSM303AGR", path, bus, device, SPIDEV_MODE3, 8 * 1000 * 1000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
LSM303AGR::LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("LSM303AGR", nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "LSM303AGR_mag_read")),
_bad_registers(perf_alloc(PC_COUNT, "LSM303AGR_bad_reg")),
_bad_values(perf_alloc(PC_COUNT, "LSM303AGR_bad_val")),
@ -83,9 +84,6 @@ LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation r
LSM303AGR::~LSM303AGR()
{
/* make sure we are truly inactive */
stop();
if (_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
}
@ -99,12 +97,12 @@ LSM303AGR::~LSM303AGR()
int
LSM303AGR::init()
{
int ret = PX4_OK;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
return PX4_ERROR;
int ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed (%i)", ret);
return ret;
}
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
@ -118,6 +116,9 @@ LSM303AGR::init()
/* fill report structures */
measure();
_measure_interval = CONVERSION_INTERVAL;
start();
return ret;
}
@ -350,17 +351,7 @@ LSM303AGR::start()
}
void
LSM303AGR::stop()
{
if (_measure_interval > 0) {
/* ensure no new items are queued while we cancel this one */
_measure_interval = 0;
ScheduleClear();
}
}
void
LSM303AGR::Run()
LSM303AGR::RunImpl()
{
if (_measure_interval == 0) {
return;
@ -476,8 +467,9 @@ LSM303AGR::collect()
}
void
LSM303AGR::print_info()
LSM303AGR::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_bad_values);

47
src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp
View File

@ -31,8 +31,7 @@
*
****************************************************************************/
#ifndef DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_
#define DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_
#pragma once
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
@ -42,7 +41,7 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <systemlib/err.h>
// Register mapping
@ -88,23 +87,26 @@ static constexpr uint8_t OUTY_H_REG_M = 0x6B;
static constexpr uint8_t OUTZ_L_REG_M = 0x6C;
static constexpr uint8_t OUTZ_H_REG_M = 0x6D;
class LSM303AGR : public device::SPI, public px4::ScheduledWorkItem
class LSM303AGR : public device::SPI, public I2CSPIDriver<LSM303AGR>
{
public:
LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation);
LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
virtual ~LSM303AGR();
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
int init() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void print_status() override;
void RunImpl();
protected:
virtual int probe();
int probe() override;
private:
@ -137,11 +139,6 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
@ -164,21 +161,6 @@ private:
*/
int collect();
/**
* 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;
/**
* Read a register from the LSM303AGR
*
@ -200,4 +182,3 @@ private:
LSM303AGR operator=(const LSM303AGR &);
};
#endif /* DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_ */

155
src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp
View File

@ -33,149 +33,74 @@
#include "LSM303AGR.hpp"
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#define LSM303AGR_DEVICE_PATH_MAG "/dev/lsm303agr_mag"
extern "C" { __EXPORT int lsm303agr_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
namespace lsm303agr
{
LSM303AGR *g_dev;
void start(enum Rotation rotation);
void info();
void usage();
/**
* Start the driver.
*
* This function call only returns once the driver is
* up and running or failed to detect the sensor.
*/
void
start(enum Rotation rotation)
LSM303AGR::print_usage()
{
int fd_mag = -1;
PRINT_MODULE_USAGE_NAME("lsm303agr", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
if (g_dev != nullptr) {
errx(0, "already started");
}
I2CSPIDriverBase *LSM303AGR::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
LSM303AGR *instance = new LSM303AGR(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
/* create the driver */
#if defined(PX4_SPIDEV_LSM303A_M) && defined(PX4_SPIDEV_LSM303A_X)
g_dev = new LSM303AGR(PX4_SPI_BUS_SENSOR5, LSM303AGR_DEVICE_PATH_MAG, PX4_SPIDEV_LSM303A_M, rotation);
#else
errx(0, "External SPI not available");
#endif
if (g_dev == nullptr) {
if (!instance) {
PX4_ERR("alloc failed");
goto fail;
return nullptr;
}
if (OK != g_dev->init()) {
goto fail;
if (OK != instance->init()) {
delete instance;
return nullptr;
}
fd_mag = px4_open(LSM303AGR_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
}
px4_close(fd_mag);
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
return instance;
}
/**
* Print a little info about the driver.
*/
void
info()
extern "C" int lsm303agr_main(int argc, char *argv[])
{
if (g_dev == nullptr) {
errx(1, "driver not running\n");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
void
usage()
{
PX4_INFO("missing command: try 'start', 'info', 'reset'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
}
} // namespace
int
lsm303agr_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
using ThisDriver = LSM303AGR;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 8 * 1000 * 1000;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
lsm303agr::usage();
exit(0);
}
}
if (myoptind >= argc) {
lsm303agr::usage();
exit(0);
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LSM303AGR);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
lsm303agr::start(rotation);
return ThisDriver::module_start(cli, iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
lsm303agr::info();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
errx(1, "unrecognized command, try 'start', 'info'");
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}