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

refactor l3gd20: use driver base class

This commit is contained in:
Beat Küng 2020-03-18 17:34:55 +01:00 committed by Daniel Agar
parent 6cf4de9e02
commit 609eafd4cd
6 changed files with 89 additions and 174 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

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

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

@ -70,8 +70,8 @@ then
# sensor heating is available, but we disable it for now
param set SENS_EN_THERMAL 0
# external L3GD20H is rotated 180 degrees yaw
l3gd20 -X -R 4 start
# (external) L3GD20H is rotated 180 degrees yaw
l3gd20 -s -R 4 start
# (external) LSM303D is rotated 270 degrees yaw
lsm303d -s -R 6 start
@ -105,7 +105,7 @@ else
# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
fi
l3gd20 start
l3gd20 -s start
lsm303d -s start
fi

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

@ -78,8 +78,8 @@ then
ak09916 -X -R 6 start
fi
# external L3GD20H is rotated 180 degrees yaw
l3gd20 -X -R 4 start
# (external) L3GD20H is rotated 180 degrees yaw
l3gd20 -s -R 4 start
# (external) LSM303D is rotated 270 degrees yaw
lsm303d -s -R 6 start
@ -113,7 +113,7 @@ else
# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
fi
l3gd20 start
l3gd20 -s start
lsm303d -s start
fi

32
src/drivers/imu/l3gd20/L3GD20.cpp
View File

@ -35,9 +35,10 @@
constexpr uint8_t L3GD20::_checked_registers[];
L3GD20::L3GD20(int bus, uint32_t device, enum Rotation rotation) :
SPI("L3GD20", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("L3GD20", nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_errors(perf_alloc(PC_COUNT, MODULE_NAME": err")),
@ -49,10 +50,6 @@ L3GD20::L3GD20(int bus, uint32_t device, enum Rotation rotation) :
L3GD20::~L3GD20()
{
/* make sure we are truly inactive */
stop();
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_errors);
perf_free(_bad_registers);
@ -226,20 +223,11 @@ L3GD20::set_samplerate(unsigned frequency)
void
L3GD20::start()
{
/* make sure we are stopped first */
stop();
/* start polling at the specified rate */
uint64_t interval = 1000000 / L3GD20_DEFAULT_RATE;
ScheduleOnInterval(interval - L3GD20_TIMER_REDUCTION, 10000);
}
void
L3GD20::stop()
{
ScheduleClear();
}
void
L3GD20::disable_i2c()
{
@ -288,13 +276,6 @@ L3GD20::reset()
_read = 0;
}
void
L3GD20::Run()
{
/* make another measurement */
measure();
}
void
L3GD20::check_registers()
{
@ -326,7 +307,7 @@ L3GD20::check_registers()
}
void
L3GD20::measure()
L3GD20::RunImpl()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
@ -412,8 +393,9 @@ L3GD20::measure()
}
void
L3GD20::print_info()
L3GD20::print_status()
{
I2CSPIDriverBase::print_status();
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_errors);

32
src/drivers/imu/l3gd20/L3GD20.hpp
View File

@ -44,7 +44,7 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
/* Orientation on board */
#define SENSOR_BOARD_ROTATION_000_DEG 0
@ -158,18 +158,22 @@
*/
#define L3GD20_TIMER_REDUCTION 600
class L3GD20 : public device::SPI, public px4::ScheduledWorkItem
class L3GD20 : public device::SPI, public I2CSPIDriver<L3GD20>
{
public:
L3GD20(int bus, uint32_t device, enum Rotation rotation);
virtual ~L3GD20();
L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
~L3GD20() override;
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void RunImpl();
int init() override;
void print_status() override;
// print register dump
void print_registers();
@ -178,7 +182,8 @@ public:
void test_error();
protected:
virtual int probe();
void custom_method(const BusCLIArguments &cli) override;
int probe() override;
private:
@ -217,21 +222,14 @@ private:
uint8_t _checked_next{0};
void start();
void stop();
void reset();
void disable_i2c();
void Run() override;
/**
* check key registers for correct values
*/
void check_registers();
/**
* Fetch measurements from the sensor and update the report ring.
*/
void measure();
/**
* Read a register from the L3GD20
*

185
src/drivers/imu/l3gd20/l3gd20_main.cpp
View File

@ -33,161 +33,96 @@
#include "L3GD20.hpp"
/**
* Local functions in support of the shell command.
*/
namespace l3gd20
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void
L3GD20::print_usage()
{
L3GD20 *g_dev{nullptr};
/**
* Start the driver.
*
* This function call only returns once the driver
* started or failed to detect the sensor.
*/
static void start(bool external_bus, enum Rotation rotation)
{
if (g_dev != nullptr) {
errx(0, "already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO)
g_dev = new L3GD20(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_GYRO, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_GYRO, rotation);
}
if (g_dev == nullptr) {
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_MODULE_USAGE_NAME("l3gd20", "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_COMMAND("regdump");
PRINT_MODULE_USAGE_COMMAND("testerror");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
/**
* Print a little info about the driver.
*/
static void info()
I2CSPIDriverBase *L3GD20::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
errx(1, "driver not running\n");
L3GD20 *instance = new L3GD20(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
/**
* Dump the register information
*/
static void regdump()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
if (OK != instance->init()) {
delete instance;
return nullptr;
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
return instance;
}
/**
* trigger an error
*/
static void test_error()
void L3GD20::custom_method(const BusCLIArguments &cli)
{
if (g_dev == nullptr) {
errx(1, "driver not running");
switch (cli.custom1) {
case 0: print_registers(); break;
case 1: test_error(); break;
}
printf("regdump @ %p\n", g_dev);
g_dev->test_error();
exit(0);
}
static void usage()
extern "C" int l3gd20_main(int argc, char *argv[])
{
warnx("missing command: try 'start', 'info', 'testerror' or 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
extern "C" __EXPORT int l3gd20_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool external_bus = false;
enum Rotation rotation = ROTATION_NONE;
using ThisDriver = L3GD20;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 11 * 1000 * 1000;
while ((ch = px4_getopt(argc, argv, "XR:", &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;
default:
l3gd20::usage();
return 0;
}
}
if (myoptind >= argc) {
l3gd20::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_GYR_DEVTYPE_L3GD20);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
l3gd20::start(external_bus, rotation);
} else if (!strcmp(verb, "info")) {
l3gd20::info();
} else if (!strcmp(verb, "regdump")) {
l3gd20::regdump();
} else if (!strcmp(verb, "testerror")) {
l3gd20::test_error();
return ThisDriver::module_start(cli, iterator);
}
l3gd20::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "regdump")) {
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "testerror")) {
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}