fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 23:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor fxas21002c: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-23 11:21:16 +01:00
committed by Daniel Agar
parent 6fd027c4dd
commit 5fa4cd1019
4 changed files with 88 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/nxp/fmuk66-v3/init/rc.board_sensors
+1 -1
View File
@@ -22,4 +22,4 @@ mpl3115a2 -I start
fxos8701cq start
# Internal SPI (gyro)
fxas21002c start
fxas21002c -s start
src/drivers/imu/fxas21002c/FXAS21002C.cpp
+7 -25
View File
@@ -186,9 +186,10 @@ static constexpr uint8_t _checked_registers[] {
using namespace time_literals;
FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) :
SPI("FXAS21002C", nullptr, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
FXAS21002C::FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("FXAS21002C", 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(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_errors(perf_alloc(PC_COUNT, MODULE_NAME": err")),
@@ -200,10 +201,6 @@ FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) :
FXAS21002C::~FXAS21002C()
{
/* make sure we are truly inactive */
stop();
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_errors);
perf_free(_bad_registers);
@@ -471,26 +468,10 @@ FXAS21002C::set_onchip_lowpass_filter(int frequency_hz)
void
FXAS21002C::start()
{
/* make sure we are stopped first */
stop();
/* start polling at the specified rate */
ScheduleOnInterval((1_s / FXAS21002C_DEFAULT_RATE) - FXAS21002C_TIMER_REDUCTION, 10000);
}
void
FXAS21002C::stop()
{
ScheduleClear();
}
void
FXAS21002C::Run()
{
/* make another measurement */
measure();
}
void
FXAS21002C::check_registers(void)
{
@@ -522,7 +503,7 @@ FXAS21002C::check_registers(void)
}
void
FXAS21002C::measure()
FXAS21002C::RunImpl()
{
// start the performance counter
perf_begin(_sample_perf);
@@ -589,8 +570,9 @@ FXAS21002C::measure()
}
void
FXAS21002C::print_info()
FXAS21002C::print_status()
{
I2CSPIDriverBase::print_status();
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_errors);
src/drivers/imu/fxas21002c/FXAS21002C.hpp
+14 -29
View File
@@ -43,34 +43,31 @@
#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>
class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem
class FXAS21002C : public device::SPI, public I2CSPIDriver<FXAS21002C>
{
public:
FXAS21002C(int bus, uint32_t device, enum Rotation rotation);
FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
virtual ~FXAS21002C();
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();
int init() override;
/**
* dump register values
*/
void print_registers();
void print_status() override;
/**
* deliberately trigger an error
*/
void test_error();
void RunImpl();
void print_registers();
void test_error();
protected:
virtual int probe();
void custom_method(const BusCLIArguments &cli);
int probe() override;
private:
@@ -101,11 +98,6 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
@@ -118,18 +110,11 @@ private:
*/
void set_standby(int rate, bool standby_true);
void Run() override;
/**
* check key registers for correct values
*/
void check_registers(void);
/**
* Fetch accel measurements from the sensor and update the report ring.
*/
void measure();
/**
* Read a register from the FXAS21002C
*
src/drivers/imu/fxas21002c/fxas21002c_main.cpp
+66 -155
View File
@@ -33,187 +33,98 @@
#include "FXAS21002C.hpp"
extern "C" { __EXPORT int fxas21002c_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
namespace fxas21002c
{
FXAS21002C *g_dev;
void start(bool external_bus, enum Rotation rotation);
void info();
void regdump();
void usage();
void test_error();
/**
* 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)
{
if (g_dev != nullptr) {
PX4_INFO("already started");
exit(0);
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO)
g_dev = new FXAS21002C(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_GYRO, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
#endif
} else {
g_dev = new FXAS21002C(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_GYRO, rotation);
}
if (g_dev == nullptr) {
PX4_ERR("failed instantiating FXAS21002C 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) {
PX4_ERR("driver not running\n");
exit(1);
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
/**
* dump registers from device
*/
void
regdump()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
}
/**
* trigger an error
*/
void
test_error()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
g_dev->test_error();
exit(0);
}
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void
usage()
FXAS21002C::print_usage()
{
PX4_INFO("missing command: try 'start', 'info', 'testerror' or 'regdump'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
PRINT_MODULE_USAGE_NAME("fxas21002c", "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();
}
} // namespace
int
fxas21002c_main(int argc, char *argv[])
I2CSPIDriverBase *FXAS21002C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
bool external_bus = false;
enum Rotation rotation = ROTATION_NONE;
FXAS21002C *instance = new FXAS21002C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
int ch = 0;
int myoptind = 1;
const char *myoptarg = NULL;
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return instance;
}
void FXAS21002C::custom_method(const BusCLIArguments &cli)
{
switch (cli.custom1) {
case 0: print_registers(); break;
case 1: test_error(); break;
}
}
extern "C" int fxas21002c_main(int argc, char *argv[])
{
int ch;
using ThisDriver = FXAS21002C;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 2 * 1000 * 1000;
cli.spi_mode = SPIDEV_MODE0;
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:
fxas21002c::usage();
return 0;
}
}
const char *verb = argv[myoptind];
const char *verb = cli.optarg();
/*
* Start/load the driver.
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_GYR_DEVTYPE_FXAS2100C);
*/
if (!strcmp(verb, "start")) {
fxas21002c::start(external_bus, rotation);
return ThisDriver::module_start(cli, iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
fxas21002c::info();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
/*
* dump device registers
*/
if (!strcmp(verb, "regdump")) {
fxas21002c::regdump();
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
/*
* trigger an error
*/
if (!strcmp(verb, "testerror")) {
fxas21002c::test_error();
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
PX4_WARN("unrecognized command, try 'start', 'info', 'testerror' or 'regdump'");
ThisDriver::print_usage();
return -1;
}