Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

refactor icm20948: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-18 17:46:55 +01:00
committed by Daniel Agar
parent 924f46ee28
commit 6588dd0861
10 changed files with 98 additions and 334 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/nxp/fmurt1062-v1/init/rc.board_sensors
+1 -1
View File
@@ -36,7 +36,7 @@ qmc5883 -X start
lis3mdl -X start
# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
if ! icm20948 -X -M -R 6 start
if ! icm20948 -X -R 6 start
then
# external emulated AK09916 (Here2) is rotated 270 degrees yaw
ak09916 -X -R 6 start
boards/px4/fmu-v3/init/rc.board_sensors
+1 -1
View File
@@ -72,7 +72,7 @@ then
param set SENS_EN_THERMAL 0
# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
if ! icm20948 -X -M -R 6 start
if ! icm20948 -X -R 6 start
then
# external emulated AK09916 (Here2) is rotated 270 degrees yaw
ak09916 -X -R 6 start
boards/px4/fmu-v5/init/rc.board_sensors
+1 -1
View File
@@ -30,7 +30,7 @@ qmc5883 -X start
lis3mdl -X start
# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
if ! icm20948 -X -M -R 6 start
if ! icm20948 -X -R 6 start
then
# external emulated AK09916 (Here2) is rotated 270 degrees yaw
ak09916 -X -R 6 start
src/drivers/imu/icm20948/ICM20948_mag.h
+1 -3
View File
@@ -101,9 +101,7 @@ struct ak09916_regs {
};
#pragma pack(pop)
extern device::Device *AK09916_I2C_interface(int bus);
typedef device::Device *(*ICM20948_mag_constructor)(int, bool);
extern device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
/**
* Helper class implementing the magnetometer driver node.
src/drivers/imu/icm20948/icm20948.cpp
+7 -39
View File
@@ -84,8 +84,10 @@ const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGIST
ICMREG_20948_ACCEL_CONFIG_2
};
ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) :
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation,
I2CSPIBusOption bus_option,
int bus) :
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
_interface(interface),
_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
_px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
@@ -106,10 +108,6 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enu
ICM20948::~ICM20948()
{
// make sure we are truly inactive
stop();
// delete the perf counter
perf_free(_sample_perf);
perf_free(_interval_perf);
perf_free(_bad_transfers);
@@ -148,15 +146,12 @@ ICM20948::init()
/* Magnetometer setup */
#ifdef USE_I2C
px4_usleep(100);
if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak09916 interface");
}
#endif /* USE_I2C */
ret = _mag.ak09916_reset();
if (ret != OK) {
@@ -235,11 +230,7 @@ ICM20948::reset_mpu()
// INT CFG => Interrupt on Data Ready
write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN); // INT: Raw data ready
#ifdef USE_I2C
bool bypass = !_mag.is_passthrough();
#else
bool bypass = false;
#endif
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
@@ -602,25 +593,9 @@ ICM20948::set_accel_range(unsigned max_g_in)
void
ICM20948::start()
{
/* make sure we are stopped first */
stop();
ScheduleOnInterval(_call_interval - ICM20948_TIMER_REDUCTION, 1000);
}
void
ICM20948::stop()
{
ScheduleClear();
}
void
ICM20948::Run()
{
/* make another measurement */
measure();
}
void
ICM20948::check_registers(void)
{
@@ -728,7 +703,7 @@ ICM20948::check_duplicate(uint8_t *accel_data)
}
void
ICM20948::measure()
ICM20948::RunImpl()
{
perf_begin(_sample_perf);
perf_count(_interval_perf);
@@ -777,21 +752,13 @@ ICM20948::measure()
* try to read a magnetometer report.
*/
# ifdef USE_I2C
if (_mag.is_passthrough()) {
# endif
_mag._measure(timestamp_sample, mpu_report.mag);
# ifdef USE_I2C
} else {
_mag.measure();
}
# endif
// Continue evaluating gyro and accelerometer results
if (_register_wait == 0) {
// Convert from big to little endian
@@ -854,8 +821,9 @@ ICM20948::measure()
}
void
ICM20948::print_info()
ICM20948::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
src/drivers/imu/icm20948/icm20948.h
+15 -27
View File
@@ -37,16 +37,12 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.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 <lib/systemlib/conversions.h>
#include <lib/systemlib/px4_macros.h>
#include "ICM20948_mag.h"
#if defined(PX4_I2C_OBDEV_ICM20948) || defined(PX4_I2C_BUS_EXPANSION)
# define USE_I2C
#endif
// ICM20948 registers
#define MPUREG_WHOAMI 0x75
@@ -339,40 +335,38 @@ struct MPUReport {
# define ICM20948_LOW_SPEED_OP(r) ((r) &~ICM20948_HIGH_BUS_SPEED)
/* interface factories */
extern device::Device *ICM20948_SPI_interface(int bus, uint32_t cs);
extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address);
extern int ICM20948_probe(device::Device *dev);
typedef device::Device *(*ICM20948_constructor)(int, uint32_t);
extern device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency);
class ICM20948_mag;
class ICM20948 : public px4::ScheduledWorkItem
class ICM20948 : public I2CSPIDriver<ICM20948>
{
public:
ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation);
ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option,
int bus);
virtual ~ICM20948();
virtual int init();
uint8_t get_whoami() { return _whoami; }
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();
void print_status() override;
void RunImpl();
protected:
device::Device *_interface;
uint8_t _whoami{0}; /** whoami result */
virtual int probe();
int probe();
friend class ICM20948_mag;
private:
void Run() override;
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
@@ -423,7 +417,6 @@ private:
bool _got_duplicate{false};
void start();
void stop();
int reset();
/**
@@ -431,11 +424,6 @@ private:
*/
int reset_mpu();
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Select a register bank in ICM20948
*
src/drivers/imu/icm20948/icm20948_i2c.cpp
+6 -10
View File
@@ -43,14 +43,12 @@
#include "icm20948.h"
#ifdef USE_I2C
device::Device *ICM20948_I2C_interface(int bus, uint32_t address);
device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency);
class ICM20948_I2C : public device::I2C
{
public:
ICM20948_I2C(int bus, uint32_t address);
ICM20948_I2C(int bus, uint32_t address, int bus_frequency);
~ICM20948_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -64,13 +62,13 @@ private:
};
device::Device *
ICM20948_I2C_interface(int bus, uint32_t address)
ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency)
{
return new ICM20948_I2C(bus, address);
return new ICM20948_I2C(bus, address, bus_frequency);
}
ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address) :
I2C("ICM20948_I2C", nullptr, bus, address, 400000)
ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address, int bus_frequency) :
I2C("ICM20948_I2C", nullptr, bus, address, bus_frequency)
{
_device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948;
}
@@ -126,5 +124,3 @@ ICM20948_I2C::probe()
return -ENODEV;
}
#endif /* USE_I2C */
src/drivers/imu/icm20948/icm20948_main.cpp
+54 -232
View File
@@ -35,284 +35,106 @@
* @file main.cpp
*
* Driver for the Invensense icm20948 connected via I2C or SPI.
*
* based on the icm20948 driver
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/conversions.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include "icm20948.h"
/** driver 'main' command */
extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); }
enum ICM20948_BUS {
ICM20948_BUS_ALL = 0,
ICM20948_BUS_I2C_INTERNAL,
ICM20948_BUS_I2C_EXTERNAL,
ICM20948_BUS_SPI_INTERNAL,
ICM20948_BUS_SPI_EXTERNAL
};
/**
* Local functions in support of the shell command.
*/
namespace icm20948
void
ICM20948::print_usage()
{
// list of supported bus configurations
struct icm20948_bus_option {
enum ICM20948_BUS busid;
ICM20948_constructor interface_constructor;
bool magpassthrough;
uint8_t busnum;
uint32_t address;
ICM20948 *dev;
} bus_options[] = {
#if defined(PX4_SPIDEV_ICM_20948) && defined(PX4_SPI_BUS_1)
{ ICM20948_BUS_SPI_INTERNAL, &ICM20948_SPI_interface, true, PX4_SPI_BUS_1, PX4_SPIDEV_ICM_20948, nullptr },
#endif
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_EXPANSION)
{ ICM20948_BUS_I2C_EXTERNAL, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr },
# endif
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
bool start_bus(icm20948_bus_option &bus, enum Rotation rotation);
icm20948_bus_option *find_bus(enum ICM20948_BUS busid);
int start(enum ICM20948_BUS busid, enum Rotation rotation);
int stop(enum ICM20948_BUS busid);
int info(enum ICM20948_BUS busid);
int usage();
/**
* find a bus structure for a busid
*/
struct icm20948_bus_option *find_bus(enum ICM20948_BUS busid)
{
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == ICM20948_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != nullptr) {
return &bus_options[i];
}
}
PX4_ERR("bus %u not started", (unsigned)busid);
return nullptr;
PRINT_MODULE_USAGE_NAME("icm20948", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
/**
* start driver for a specific bus option
*/
bool
start_bus(icm20948_bus_option &bus, enum Rotation rotation)
I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
PX4_INFO("Bus probed: %d", bus.busid);
device::Device *interface = nullptr;
device::Device *mag_interface = nullptr;
if (bus.dev != nullptr) {
PX4_ERR("SPI %d not available", bus.busid);
return false;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = ICM20948_I2C_interface(iterator.bus(), PX4_I2C_EXT_ICM20948_1, cli.bus_frequency);
// For i2c interfaces, connect to the magnetometer directly
mag_interface = AK09916_I2C_interface(iterator.bus(), cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = ICM20948_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
}
device::Device *interface = bus.interface_constructor(bus.busnum, bus.address);
if (interface == nullptr) {
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
PX4_ERR("alloc failed");
delete mag_interface;
return nullptr;
}
if (interface->init() != OK) {
delete interface;
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
delete mag_interface;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
device::Device *mag_interface = nullptr;
ICM20948 *dev = new ICM20948(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
#ifdef USE_I2C
/* For i2c interfaces, connect to the magnetomer directly */
const bool is_i2c = bus.busid == ICM20948_BUS_I2C_INTERNAL || bus.busid == ICM20948_BUS_I2C_EXTERNAL;
if (is_i2c) {
mag_interface = AK09916_I2C_interface(bus.busnum);
}
#endif
bus.dev = new ICM20948(interface, mag_interface, rotation);
if (bus.dev == nullptr) {
if (dev == nullptr) {
delete interface;
if (mag_interface != nullptr) {
delete mag_interface;
}
return false;
delete mag_interface;
return nullptr;
}
if (OK != bus.dev->init()) {
goto fail;
if (OK != dev->init()) {
delete dev;
return nullptr;
}
return true;
fail:
if (bus.dev != nullptr) {
delete (bus.dev);
bus.dev = nullptr;
}
PX4_ERR("driver start failed");
return false;
return dev;
}
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
int
start(enum ICM20948_BUS busid, enum Rotation rotation)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].dev != nullptr) {
// this device is already started
continue;
}
if (busid != ICM20948_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
if (started) { break; }
}
return PX4_OK;
}
int
stop(enum ICM20948_BUS busid)
{
icm20948_bus_option *bus = find_bus(busid);
if (bus != nullptr && bus->dev != nullptr) {
delete bus->dev;
bus->dev = nullptr;
} else {
/* warn, but not an error */
PX4_WARN("already stopped.");
}
return PX4_OK;
}
/**
* Print a little info about the driver.
*/
int
info(enum ICM20948_BUS busid)
{
icm20948_bus_option *bus = find_bus(busid);
if (bus != nullptr && bus->dev != nullptr) {
PX4_WARN("driver not running");
return PX4_ERROR;
}
bus->dev->print_info();
return PX4_OK;
}
int
usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'info'");
PX4_INFO("options:");
PX4_INFO(" -X (i2c external bus)");
PX4_INFO(" -I (i2c internal bus)");
PX4_INFO(" -s (spi internal bus)");
PX4_INFO(" -S (spi external bus)");
PX4_INFO(" -t (spi internal bus, 2nd instance)");
PX4_INFO(" -R rotation");
return PX4_OK;
}
} // namespace icm20948
int
extern "C" int
icm20948_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
using ThisDriver = ICM20948;
BusCLIArguments cli{true, true};
cli.default_spi_frequency = 1000 * 1000; // low speed freq
cli.default_i2c_frequency = 400000;
enum ICM20948_BUS busid = ICM20948_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'X':
busid = ICM20948_BUS_I2C_EXTERNAL;
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
return icm20948::usage();
}
}
if (myoptind >= argc) {
return icm20948::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_ICM20948);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
return icm20948::start(busid, rotation);
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return icm20948::stop(busid);
return ThisDriver::module_stop(iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
return icm20948::info(busid);
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
return icm20948::usage();
ThisDriver::print_usage();
return -1;
}
src/drivers/imu/icm20948/icm20948_spi.cpp
+6 -10
View File
@@ -59,12 +59,12 @@
#define ICM20948_LOW_SPI_BUS_SPEED 1000*1000
#define ICM20948_HIGH_SPI_BUS_SPEED 20*1000*1000
device::Device *ICM20948_SPI_interface(int bus, uint32_t cs);
device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
class ICM20948_SPI : public device::SPI
{
public:
ICM20948_SPI(int bus, uint32_t device);
ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
~ICM20948_SPI() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -80,17 +80,13 @@ private:
};
device::Device *
ICM20948_SPI_interface(int bus, uint32_t cs)
ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
{
device::Device *interface = nullptr;
interface = new ICM20948_SPI(bus, cs);
return interface;
return new ICM20948_SPI(bus, devid, bus_frequency, spi_mode);
}
ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device) :
SPI("ICM20948", nullptr, bus, device, SPIDEV_MODE3, ICM20948_LOW_SPI_BUS_SPEED)
ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
SPI("ICM20948", nullptr, bus, device, spi_mode, bus_frequency)
{
_device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948;
}
src/drivers/imu/icm20948/mag_i2c.cpp
+6 -10
View File
@@ -42,14 +42,12 @@
#include <drivers/device/i2c.h>
#ifdef USE_I2C
device::Device *AK09916_I2C_interface(int bus);
device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
class AK09916_I2C : public device::I2C
{
public:
AK09916_I2C(int bus);
AK09916_I2C(int bus, int bus_frequency);
~AK09916_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -61,13 +59,13 @@ protected:
};
device::Device *
AK09916_I2C_interface(int bus)
AK09916_I2C_interface(int bus, int bus_frequency)
{
return new AK09916_I2C(bus);
return new AK09916_I2C(bus, bus_frequency);
}
AK09916_I2C::AK09916_I2C(int bus) :
I2C("AK09916_I2C", nullptr, bus, AK09916_I2C_ADDR, 400000)
AK09916_I2C::AK09916_I2C(int bus, int bus_frequency) :
I2C("AK09916_I2C", nullptr, bus, AK09916_I2C_ADDR, bus_frequency)
{
_device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948;
}
@@ -109,5 +107,3 @@ AK09916_I2C::probe()
return OK;
}
#endif // USE_I2C