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

refactor bmi160: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-17 18:07:26 +01:00
committed by Daniel Agar
parent 54da4997ad
commit 785f18ebf8
4 changed files with 83 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_sensor.h
-1
View File
@@ -77,7 +77,6 @@
#define DRV_IMU_DEVTYPE_MPU9250 0x24
#define DRV_GYR_DEVTYPE_BMI160 0x25
#define DRV_IMU_DEVTYPE_ICM42688P 0x26
#define DRV_IMU_DEVTYPE_ICM40609D 0x27
#define DRV_RNG_DEVTYPE_MB12XX 0x31
src/drivers/imu/bmi160/bmi160.cpp
+9 -26
View File
@@ -49,9 +49,10 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM
BMIREG_NV_CONF
};
BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
SPI("BMI160", nullptr, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI("BMI160", 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(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")),
@@ -63,15 +64,12 @@ BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
_reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates"))
{
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_BMI160);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_BMI160);
}
BMI160::~BMI160()
{
/* make sure we are truly inactive */
stop();
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_accel_reads);
@@ -494,28 +492,12 @@ BMI160::set_gyro_range(unsigned max_dps)
void
BMI160::start()
{
/* make sure we are stopped first */
stop();
/* start polling at the specified rate */
ScheduleOnInterval((1_s / BMI160_GYRO_DEFAULT_RATE) - BMI160_TIMER_REDUCTION, 1000);
reset();
}
void
BMI160::stop()
{
ScheduleClear();
}
void
BMI160::Run()
{
/* make another measurement */
measure();
}
void
BMI160::check_registers(void)
{
@@ -560,7 +542,7 @@ BMI160::check_registers(void)
}
void
BMI160::measure()
BMI160::RunImpl()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
@@ -690,8 +672,9 @@ BMI160::measure()
}
void
BMI160::print_info()
BMI160::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
src/drivers/imu/bmi160/bmi160.hpp
+15 -22
View File
@@ -40,7 +40,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>
#include <systemlib/conversions.h>
#define DIR_READ 0x80
@@ -243,33 +243,38 @@
using namespace time_literals;
class BMI160 : public device::SPI, public px4::ScheduledWorkItem
class BMI160 : public device::SPI, public I2CSPIDriver<BMI160>
{
public:
BMI160(int bus, uint32_t device, enum Rotation rotation);
BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
virtual ~BMI160();
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;
void print_status() override;
void print_registers();
// deliberately cause a sensor error
void test_error();
void RunImpl();
protected:
virtual int probe();
int probe() override;
void custom_method(const BusCLIArguments &cli) override;
private:
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
uint8_t _whoami; /** whoami result */
uint8_t _whoami; ///< whoami result
unsigned _dlpf_freq;
@@ -306,11 +311,6 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
@@ -318,13 +318,6 @@ private:
*/
int reset();
void Run() override;
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Read a register from the BMI160
*
src/drivers/imu/bmi160/bmi160_main.cpp
+59 -172
View File
@@ -33,221 +33,108 @@
#include "bmi160.hpp"
/** driver 'main' command */
extern "C" { __EXPORT int bmi160_main(int argc, char *argv[]); }
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/**
* Local functions in support of the shell command.
*/
namespace bmi160
{
BMI160 *g_dev_int; // on internal bus
BMI160 *g_dev_ext; // on external bus
void start(bool, enum Rotation);
void stop(bool);
void info(bool);
void regdump(bool);
void testerror(bool);
void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation)
BMI160::print_usage()
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
PRINT_MODULE_USAGE_NAME("bmi160", "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("reset");
PRINT_MODULE_USAGE_COMMAND("regdump");
PRINT_MODULE_USAGE_COMMAND("testerror");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
I2CSPIDriverBase *BMI160::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
BMI160 *instance = new BMI160(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BMI, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
#if defined(PX4_SPIDEV_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BMI, rotation);
#else
errx(0, "No Internal SPI CS");
#endif
if (OK != instance->init()) {
delete instance;
return nullptr;
}
if (*g_dev_ptr == nullptr) {
goto fail;
}
if (OK != (*g_dev_ptr)->init()) {
goto fail;
}
exit(0);
fail:
if (*g_dev_ptr != nullptr) {
delete (*g_dev_ptr);
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
return instance;
}
void
stop(bool external_bus)
BMI160::custom_method(const BusCLIArguments &cli)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
switch (cli.custom1) {
case 0: reset();
break;
if (*g_dev_ptr != nullptr) {
delete *g_dev_ptr;
*g_dev_ptr = nullptr;
case 1: print_registers();
break;
} else {
/* warn, but not an error */
warnx("already stopped.");
case 2: test_error();
break;
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info(bool external_bus)
extern "C" int bmi160_main(int argc, char *argv[])
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
/**
* Dump the register information
*/
void
regdump(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
printf("regdump @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_registers();
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(bool external_bus)
{
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->test_error();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
bmi160_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 = BMI160;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = BMI160_BUS_SPEED;
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:
bmi160::usage();
return 0;
}
}
if (myoptind >= argc) {
bmi160::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_BMI160);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
bmi160::start(external_bus, rotation);
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
bmi160::stop(external_bus);
return ThisDriver::module_stop(iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
bmi160::info(external_bus);
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "reset")) {
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
bmi160::regdump(external_bus);
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "testerror")) {
bmi160::testerror(external_bus);
cli.custom1 = 2;
return ThisDriver::module_custom_method(cli, iterator);
}
bmi160::usage();
ThisDriver::print_usage();
return -1;
}