Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 9 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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/px4_config.h>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include "mpu9250.h" #include "mpu9250.h"
/** driver 'main' command */ enum class MPU9250_BUS {
extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } ALL = 0,
I2C_INTERNAL,
enum MPU9250_BUS { I2C_EXTERNAL,
MPU9250_BUS_ALL = 0, SPI_INTERNAL,
MPU9250_BUS_I2C_INTERNAL, SPI_INTERNAL2,
MPU9250_BUS_I2C_EXTERNAL, SPI_EXTERNAL
MPU9250_BUS_SPI_INTERNAL,
MPU9250_BUS_SPI_INTERNAL2,
MPU9250_BUS_SPI_EXTERNAL
}; };
/**
* Local functions in support of the shell command.
*/
namespace mpu9250 namespace mpu9250
{ {
// list of supported bus configurations // list of supported bus configurations
struct mpu9250_bus_option { struct mpu9250_bus_option {
enum MPU9250_BUS busid; MPU9250_BUS busid;
MPU9250_constructor interface_constructor; MPU9250_constructor interface_constructor;
bool magpassthrough; bool magpassthrough;
uint8_t busnum; uint8_t busnum;
@@ -76,156 +59,102 @@ struct mpu9250_bus_option {
} bus_options[] = { } bus_options[] = {
#if defined(USE_I2C) #if defined(USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250) # 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 # endif
# if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_MPU9250) # 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 # endif
# if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250) # 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 # endif
# if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250) # 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
#endif #endif
#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU) #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 #endif
#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU2) #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 #endif
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) #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 #endif
}; };
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) // find a bus structure for a busid
static struct mpu9250_bus_option *find_bus(MPU9250_BUS busid)
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)
{ {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { for (mpu9250_bus_option &bus_option : bus_options) {
if ((busid == MPU9250_BUS_ALL || if ((busid == MPU9250_BUS::ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != nullptr) { busid == bus_option.busid) && bus_option.dev != nullptr) {
return &bus_options[i];
return &bus_option;
} }
} }
PX4_ERR("bus %u not started", (unsigned)busid);
return nullptr; return nullptr;
} }
/** static bool start_bus(mpu9250_bus_option &bus, enum Rotation rotation)
* start driver for a specific bus option
*/
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); 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); PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (interface->init() != OK) {
delete interface; delete interface;
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false; return false;
} }
device::Device *mag_interface = nullptr; device::Device *mag_interface = nullptr;
#ifdef USE_I2C #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); 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; delete interface;
delete mag_interface;
if (mag_interface != nullptr) {
delete mag_interface;
}
return false; return false;
} }
if (OK != bus.dev->init()) { bus.dev = dev;
goto fail;
}
return true; return true;
fail:
if (bus.dev != nullptr) {
delete (bus.dev);
bus.dev = nullptr;
}
PX4_ERR("driver start failed");
return false;
} }
/** static int start(MPU9250_BUS busid, enum Rotation rotation)
* 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)
{ {
bool started = false; for (mpu9250_bus_option &bus_option : bus_options) {
if (bus_option.dev != nullptr) {
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].dev != nullptr) {
// this device is already started // this device is already started
PX4_WARN("already started");
continue; 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 // not the one that is asked for
continue; continue;
} }
started |= start_bus(bus_options[i], rotation); if (start_bus(bus_option, rotation)) {
return PX4_OK;
if (started) { break; } }
} }
return PX4_OK; return PX4_ERROR;
} }
int static int stop(MPU9250_BUS busid)
stop(enum MPU9250_BUS busid)
{ {
mpu9250_bus_option *bus = find_bus(busid); mpu9250_bus_option *bus = find_bus(busid);
@@ -234,35 +163,29 @@ stop(enum MPU9250_BUS busid)
bus->dev = nullptr; bus->dev = nullptr;
} else { } 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"); PX4_WARN("driver not running");
return PX4_ERROR; return PX4_ERROR;
} }
bus->dev->print_info();
return PX4_OK; return PX4_OK;
} }
int static int status(MPU9250_BUS busid)
usage()
{ {
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("options:");
PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -X (i2c external bus)");
PX4_INFO(" -I (i2c internal bus)"); PX4_INFO(" -I (i2c internal bus)");
@@ -271,41 +194,40 @@ usage()
PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -t (spi internal bus, 2nd instance)");
PX4_INFO(" -R rotation"); PX4_INFO(" -R rotation");
return PX4_OK; return 0;
} }
} // namespace } // namespace
int extern "C" int mpu9250_main(int argc, char *argv[])
mpu9250_main(int argc, char *argv[])
{ {
int myoptind = 1; int myoptind = 1;
int ch; int ch;
const char *myoptarg = nullptr; const char *myoptarg = nullptr;
enum MPU9250_BUS busid = MPU9250_BUS_ALL; MPU9250_BUS busid = MPU9250_BUS::ALL;
enum Rotation rotation = ROTATION_NONE; enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
case 'X': case 'X':
busid = MPU9250_BUS_I2C_EXTERNAL; busid = MPU9250_BUS::I2C_EXTERNAL;
break; break;
case 'I': case 'I':
busid = MPU9250_BUS_I2C_INTERNAL; busid = MPU9250_BUS::I2C_INTERNAL;
break; break;
case 'S': case 'S':
busid = MPU9250_BUS_SPI_EXTERNAL; busid = MPU9250_BUS::SPI_EXTERNAL;
break; break;
case 's': case 's':
busid = MPU9250_BUS_SPI_INTERNAL; busid = MPU9250_BUS::SPI_INTERNAL;
break; break;
case 't': case 't':
busid = MPU9250_BUS_SPI_INTERNAL2; busid = MPU9250_BUS::SPI_INTERNAL2;
break; break;
case 'R': case 'R':
@@ -323,22 +245,14 @@ mpu9250_main(int argc, char *argv[])
const char *verb = argv[myoptind]; const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
return mpu9250::start(busid, rotation); return mpu9250::start(busid, rotation);
}
if (!strcmp(verb, "stop")) { } else if (!strcmp(verb, "stop")) {
return mpu9250::stop(busid); return mpu9250::stop(busid);
}
/* } else if (!strcmp(verb, "status")) {
* Print driver information. return mpu9250::status(busid);
*/
if (!strcmp(verb, "info")) {
return mpu9250::info(busid);
} }
return mpu9250::usage(); return mpu9250::usage();