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

refactor lsm303d: use driver base class

This commit is contained in:
Beat Küng 2020-03-18 17:33:40 +01:00 committed by Daniel Agar
parent 3423809cd9
commit 6cf4de9e02
6 changed files with 76 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/airmind/mindpx-v2/init/rc.board_sensors
View File

@ -15,7 +15,7 @@ qmc5883 -I -R 12 start
mpu6000 -s -R 8 start
mpu9250 -s -R 8 start
lsm303d -R 10 start
lsm303d -s -R 10 start
l3gd20 -R 14 start
# Internal SPI

6
boards/px4/fmu-v2/init/rc.board_sensors
View File

@ -73,8 +73,8 @@ then
# external L3GD20H is rotated 180 degrees yaw
l3gd20 -X -R 4 start
# external LSM303D is rotated 270 degrees yaw
lsm303d -X -R 6 start
# (external) LSM303D is rotated 270 degrees yaw
lsm303d -s -R 6 start
if [ $BOARD_FMUV3 = 20 ]
then
@ -106,7 +106,7 @@ else
fi
l3gd20 start
lsm303d start
lsm303d -s start
fi
unset BOARD_FMUV3

6
boards/px4/fmu-v3/init/rc.board_sensors
View File

@ -81,8 +81,8 @@ then
# external L3GD20H is rotated 180 degrees yaw
l3gd20 -X -R 4 start
# external LSM303D is rotated 270 degrees yaw
lsm303d -X -R 6 start
# (external) LSM303D is rotated 270 degrees yaw
lsm303d -s -R 6 start
if [ $BOARD_FMUV3 = 20 ]
then
@ -114,7 +114,7 @@ else
fi
l3gd20 start
lsm303d start
lsm303d -s start
fi
# External RM3100

28
src/drivers/imu/lsm303d/LSM303D.cpp
View File

@ -53,9 +53,10 @@ static constexpr uint8_t _checked_registers[] = {
ADDR_CTRL_REG7
};
LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) :
SPI("LSM303D", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
LSM303D::LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("LSM303D", nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
_px4_mag(get_device_id(), ORB_PRIO_LOW, rotation),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")),
@ -71,9 +72,6 @@ LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) :
LSM303D::~LSM303D()
{
// make sure we are truly inactive
stop();
// delete the perf counter
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
@ -89,8 +87,8 @@ LSM303D::init()
int ret = SPI::init();
if (ret != OK) {
PX4_ERR("SPI init failed (%i)", ret);
return PX4_ERROR;
DEVICE_DEBUG("SPI init failed (%i)", ret);
return ret;
}
reset();
@ -407,21 +405,12 @@ LSM303D::mag_set_samplerate(unsigned frequency)
void
LSM303D::start()
{
// make sure we are stopped first
stop();
// start polling at the specified rate
ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION);
}
void
LSM303D::stop()
{
ScheduleClear();
}
void
LSM303D::Run()
LSM303D::RunImpl()
{
// make another accel measurement
measureAccelerometer();
@ -581,8 +570,9 @@ LSM303D::measureMagnetometer()
}
void
LSM303D::print_info()
LSM303D::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_accel_sample_perf);
perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers);

28
src/drivers/imu/lsm303d/LSM303D.hpp
View File

@ -41,7 +41,7 @@
#include <drivers/device/spi.h>
#include <ecl/geo/geo.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
@ -138,30 +138,28 @@
*/
#define LSM303D_TIMER_REDUCTION 200
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
class LSM303D : public device::SPI, public px4::ScheduledWorkItem
class LSM303D : public device::SPI, public I2CSPIDriver<LSM303D>
{
public:
LSM303D(int bus, uint32_t device, enum Rotation rotation);
virtual ~LSM303D();
LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
~LSM303D() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
int init() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_status() override;
protected:
virtual int probe();
int probe() override;
private:
void Run() override;
void start();
void stop();
void reset();
/**

164
src/drivers/imu/lsm303d/lsm303d_main.cpp
View File

@ -39,144 +39,74 @@
#include "LSM303D.hpp"
#include <px4_platform_common/getopt.h>
/**
* Local functions in support of the shell command.
*/
namespace lsm303d
{
LSM303D *g_dev;
void start(bool external_bus, enum Rotation rotation, unsigned range);
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(bool external_bus, enum Rotation rotation, unsigned range)
{
if (g_dev != nullptr) {
errx(0, "already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation);
}
if (g_dev == nullptr) {
PX4_ERR("failed instantiating LSM303D obj");
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running\n");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
#include <px4_platform_common/module.h>
void
usage()
LSM303D::print_usage()
{
PX4_INFO("missing command: try 'start', 'info'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
PRINT_MODULE_USAGE_NAME("lsm303d", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
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();
}
} // namespace
int
lsm303d_main(int argc, char *argv[])
I2CSPIDriverBase *LSM303D::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
bool external_bus = false;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
LSM303D *instance = new LSM303D(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
int myoptind = 1;
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return instance;
}
extern "C" int lsm303d_main(int argc, char *argv[])
{
int ch;
const char *myoptarg = nullptr;
using ThisDriver = LSM303D;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 11 * 1000 * 1000;
/* 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 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
case 'a':
accel_range = atoi(myoptarg);
break;
default:
lsm303d::usage();
exit(0);
}
}
if (myoptind >= argc) {
lsm303d::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_ACC_DEVTYPE_LSM303D);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
lsm303d::start(external_bus, rotation, accel_range);
return ThisDriver::module_start(cli, iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
lsm303d::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;
}