Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

mpu9250: cleanup main

Browse Source
This commit is contained in:
Daniel Agar
2019-11-14 21:39:06 -05:00
parent cc41e5be20
commit 11b1573961
1 changed files with 70 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/mpu9250/mpu9250_main.cpp
+70 -156
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,43 +31,26 @@
*
****************************************************************************/
/**
* @file main.cpp
*
* Driver for the Invensense mpu9250 connected via I2C or SPI.
*
* @authors Andrew Tridgell
* Robert Dickenson
*
* based on the mpu6000 driver
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include "mpu9250.h"
/** driver 'main' command */
extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
enum MPU9250_BUS {
MPU9250_BUS_ALL = 0,
MPU9250_BUS_I2C_INTERNAL,
MPU9250_BUS_I2C_EXTERNAL,
MPU9250_BUS_SPI_INTERNAL,
MPU9250_BUS_SPI_INTERNAL2,
MPU9250_BUS_SPI_EXTERNAL
enum class MPU9250_BUS {
ALL = 0,
I2C_INTERNAL,
I2C_EXTERNAL,
SPI_INTERNAL,
SPI_INTERNAL2,
SPI_EXTERNAL
};
/**
* Local functions in support of the shell command.
*/
namespace mpu9250
{
// list of supported bus configurations
struct mpu9250_bus_option {
enum MPU9250_BUS busid;
MPU9250_BUS busid;
MPU9250_constructor interface_constructor;
bool magpassthrough;
uint8_t busnum;
@@ -76,156 +59,102 @@ struct mpu9250_bus_option {
} bus_options[] = {
#if defined(USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250)
{ MPU9250_BUS_I2C_INTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr },
{ MPU9250_BUS::I2C_INTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr },
# endif
# if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_MPU9250)
{ MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr },
{ MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr },
# endif
# if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250)
{ MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr },
{ MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr },
# endif
# if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250)
{ MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr },
{ MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr },
# endif
#endif
#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU)
{ MPU9250_BUS_SPI_INTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr },
{ MPU9250_BUS::SPI_INTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr },
#endif
#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU2)
{ MPU9250_BUS_SPI_INTERNAL2, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr },
{ MPU9250_BUS::SPI_INTERNAL2, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr },
#endif
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)
{ MPU9250_BUS_SPI_EXTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr },
{ MPU9250_BUS::SPI_EXTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
mpu9250_bus_option *find_bus(enum MPU9250_BUS busid);
int start(enum MPU9250_BUS busid, enum Rotation rotation);
int stop(enum MPU9250_BUS busid);
int info(enum MPU9250_BUS busid);
int usage();
/**
* find a bus structure for a busid
*/
struct mpu9250_bus_option *find_bus(enum MPU9250_BUS busid)
// find a bus structure for a busid
static struct mpu9250_bus_option *find_bus(MPU9250_BUS busid)
{
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MPU9250_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != nullptr) {
return &bus_options[i];
for (mpu9250_bus_option &bus_option : bus_options) {
if ((busid == MPU9250_BUS::ALL ||
busid == bus_option.busid) && bus_option.dev != nullptr) {
return &bus_option;
}
}
PX4_ERR("bus %u not started", (unsigned)busid);
return nullptr;
}
/**
* start driver for a specific bus option
*/
bool
start_bus(mpu9250_bus_option &bus, enum Rotation rotation)
static bool start_bus(mpu9250_bus_option &bus, enum Rotation rotation)
{
PX4_INFO("Bus probed: %d", bus.busid);
if (bus.dev != nullptr) {
PX4_ERR("SPI %d not available", bus.busid);
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum, bus.address);
if (interface == nullptr) {
if ((interface == nullptr) || (interface->init() != PX4_OK)) {
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (interface->init() != OK) {
delete interface;
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
}
device::Device *mag_interface = nullptr;
#ifdef USE_I2C
/* For i2c interfaces, connect to the magnetomer directly */
const bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL;
if (is_i2c) {
// For i2c interfaces, connect to the magnetomer directly
if ((bus.busid == MPU9250_BUS::I2C_INTERNAL) || (bus.busid == MPU9250_BUS::I2C_EXTERNAL)) {
mag_interface = AK8963_I2C_interface(bus.busnum);
}
#endif
#endif // USE_I2C
bus.dev = new MPU9250(interface, mag_interface, rotation);
MPU9250 *dev = new MPU9250(interface, mag_interface, rotation);
if (bus.dev == nullptr) {
if (dev == nullptr || (dev->init() != PX4_OK)) {
PX4_ERR("driver start failed");
delete dev;
delete interface;
if (mag_interface != nullptr) {
delete mag_interface;
}
delete mag_interface;
return false;
}
if (OK != bus.dev->init()) {
goto fail;
}
bus.dev = dev;
return true;
fail:
if (bus.dev != nullptr) {
delete (bus.dev);
bus.dev = nullptr;
}
PX4_ERR("driver start failed");
return false;
}
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
int
start(enum MPU9250_BUS busid, enum Rotation rotation)
static int start(MPU9250_BUS busid, enum Rotation rotation)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].dev != nullptr) {
for (mpu9250_bus_option &bus_option : bus_options) {
if (bus_option.dev != nullptr) {
// this device is already started
PX4_WARN("already started");
continue;
}
if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) {
if (busid != MPU9250_BUS::ALL && bus_option.busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
if (started) { break; }
if (start_bus(bus_option, rotation)) {
return PX4_OK;
}
}
return PX4_OK;
return PX4_ERROR;
}
int
stop(enum MPU9250_BUS busid)
static int stop(MPU9250_BUS busid)
{
mpu9250_bus_option *bus = find_bus(busid);
@@ -234,35 +163,29 @@ stop(enum MPU9250_BUS busid)
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 MPU9250_BUS busid)
{
mpu9250_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()
static int status(MPU9250_BUS busid)
{
PX4_INFO("missing command: try 'start', 'stop', 'info'");
mpu9250_bus_option *bus = find_bus(busid);
if (bus != nullptr && bus->dev != nullptr) {
bus->dev->print_info();
return PX4_OK;
}
PX4_WARN("driver not running");
return PX4_ERROR;
}
static int usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -X (i2c external bus)");
PX4_INFO(" -I (i2c internal bus)");
@@ -271,41 +194,40 @@ usage()
PX4_INFO(" -t (spi internal bus, 2nd instance)");
PX4_INFO(" -R rotation");
return PX4_OK;
return 0;
}
} // namespace
int
mpu9250_main(int argc, char *argv[])
extern "C" int mpu9250_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
enum MPU9250_BUS busid = MPU9250_BUS_ALL;
MPU9250_BUS busid = MPU9250_BUS::ALL;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'X':
busid = MPU9250_BUS_I2C_EXTERNAL;
busid = MPU9250_BUS::I2C_EXTERNAL;
break;
case 'I':
busid = MPU9250_BUS_I2C_INTERNAL;
busid = MPU9250_BUS::I2C_INTERNAL;
break;
case 'S':
busid = MPU9250_BUS_SPI_EXTERNAL;
busid = MPU9250_BUS::SPI_EXTERNAL;
break;
case 's':
busid = MPU9250_BUS_SPI_INTERNAL;
busid = MPU9250_BUS::SPI_INTERNAL;
break;
case 't':
busid = MPU9250_BUS_SPI_INTERNAL2;
busid = MPU9250_BUS::SPI_INTERNAL2;
break;
case 'R':
@@ -323,22 +245,14 @@ mpu9250_main(int argc, char *argv[])
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
return mpu9250::start(busid, rotation);
}
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return mpu9250::stop(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
return mpu9250::info(busid);
} else if (!strcmp(verb, "status")) {
return mpu9250::status(busid);
}
return mpu9250::usage();