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

refactor fxos8701cq: use driver base class

This commit is contained in:
Beat Küng 2020-03-23 11:21:37 +01:00 committed by Daniel Agar
parent 5fa4cd1019
commit 6a41c9e417
4 changed files with 88 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/fmuk66-v3/init/rc.board_sensors
View File

@ -19,7 +19,7 @@ bmp280 -I start
mpl3115a2 -I start
# Internal SPI (accel + mag)
fxos8701cq start
fxos8701cq -s start
# Internal SPI (gyro)
fxas21002c -s start

24
src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
View File

@ -53,9 +53,10 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] =
FXOS8701CQ_M_CTRL_REG2,
};
FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) :
SPI("FXOS8701CQ", nullptr, bus, device, SPIDEV_MODE0, 1 * 1000 * 1000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
FXOS8701CQ::FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("FXOS8701CQ", 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_LOW, rotation),
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_px4_mag(get_device_id(), ORB_PRIO_LOW, rotation),
@ -77,9 +78,6 @@ FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) :
FXOS8701CQ::~FXOS8701CQ()
{
// make sure we are truly inactive
stop();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
perf_free(_mag_sample_perf);
#endif
@ -284,19 +282,10 @@ FXOS8701CQ::accel_set_samplerate(unsigned frequency)
void
FXOS8701CQ::start()
{
// make sure we are stopped first
stop();
// start polling at the specified rate
ScheduleOnInterval(1000000 / (FXOS8701C_ACCEL_DEFAULT_RATE) - FXOS8701C_TIMER_REDUCTION, 10000);
}
void
FXOS8701CQ::stop()
{
ScheduleClear();
}
void
FXOS8701CQ::check_registers(void)
{
@ -328,7 +317,7 @@ FXOS8701CQ::check_registers(void)
}
void
FXOS8701CQ::Run()
FXOS8701CQ::RunImpl()
{
// start the performance counter
perf_begin(_accel_sample_perf);
@ -407,8 +396,9 @@ FXOS8701CQ::Run()
}
void
FXOS8701CQ::print_info()
FXOS8701CQ::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_accel_sample_perf);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)

25
src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp
View File

@ -43,7 +43,7 @@
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
@ -111,27 +111,32 @@
*/
#define FXOS8701C_TIMER_REDUCTION 240
class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem
class FXOS8701CQ : public device::SPI, public I2CSPIDriver<FXOS8701CQ>
{
public:
FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation);
FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
virtual ~FXOS8701CQ();
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
int init() override;
void print_status() override;
void RunImpl();
void print_info();
void print_registers();
void test_error();
protected:
virtual int probe();
int probe() override;
void custom_method(const BusCLIArguments &cli) override;
private:
void Run() override;
void start();
void stop();
void reset();
/**

224
src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp
View File

@ -40,191 +40,97 @@
#include "FXOS8701CQ.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/**
* Local functions in support of the shell command.
*/
namespace fxos8701cq
void
FXOS8701CQ::print_usage()
{
FXOS8701CQ *g_dev{nullptr};
int start(bool external_bus, enum Rotation rotation);
int info();
int stop();
int regdump();
int usage();
int test_error();
/**
* Start the driver.
*
* This function call only returns once the driver is
* up and running or failed to detect the sensor.
*/
int
start(bool external_bus, enum Rotation rotation)
{
if (g_dev != nullptr) {
PX4_INFO("already started");
return 0;
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
PX4_ERR("External SPI not available");
return 0;
#endif
} else {
g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation);
}
if (g_dev == nullptr) {
PX4_ERR("failed instantiating FXOS8701C obj");
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("driver start failed");
return PX4_ERROR;
PRINT_MODULE_USAGE_NAME("fxos8701cq", "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.
*/
int
info()
I2CSPIDriverBase *FXOS8701CQ::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
return 1;
FXOS8701CQ *instance = new FXOS8701CQ(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
g_dev->print_info();
return 0;
}
int
stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
return 1;
if (OK != instance->init()) {
delete instance;
return nullptr;
}
delete g_dev;
g_dev = nullptr;
return 0;
return instance;
}
/**
* dump registers from device
*/
int
regdump()
void FXOS8701CQ::custom_method(const BusCLIArguments &cli)
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
return 1;
switch (cli.custom1) {
case 0: print_registers(); break;
case 1: test_error(); break;
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
return 0;
}
/**
* trigger an error
*/
int
test_error()
extern "C" int fxos8701cq_main(int argc, char *argv[])
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
return 1;
}
g_dev->test_error();
return 0;
}
int
usage()
{
PX4_INFO("missing command: try 'start', 'info', 'stop', 'testerror' or 'regdump'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
return 0;
}
} // namespace
extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
int fxos8701cq_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
using ThisDriver = FXOS8701CQ;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 1 * 1000 * 1000;
cli.spi_mode = SPIDEV_MODE0;
int myoptind = 1;
const char *myoptarg = NULL;
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;
default:
fxos8701cq::usage();
exit(0);
}
}
const char *verb = argv[myoptind];
const char *verb = cli.optarg();
if (!strcmp(verb, "start")) {
return fxos8701cq::start(external_bus, rotation);
} else if (!strcmp(verb, "stop")) {
return fxos8701cq::stop();
} else if (!strcmp(verb, "info")) {
return fxos8701cq::info();
} else if (!strcmp(verb, "regdump")) {
return fxos8701cq::regdump();
} else if (!strcmp(verb, "testerror")) {
return fxos8701cq::test_error();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
PX4_ERR("unrecognized command, try 'start', 'stop', 'info', 'testerror' or 'regdump'");
return PX4_ERROR;
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_ACC_DEVTYPE_FXOS8701C);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
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;
}