dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ADIS IMU drivers cleanup and standardize main

This commit is contained in:
Daniel Agar 2019-11-05 10:10:34 -05:00
parent 1a79f75f94
commit 5ec74af421
3 changed files with 141 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
src/drivers/imu/adis16448/adis16448_main.cpp
View File

@ -31,136 +31,112 @@
*
****************************************************************************/
/**
* @file ADIS16448.cpp
*/
#include "ADIS16448.h"
/**
* Local functions in support of the shell command.
*/
#include <px4_platform_common/getopt.h>
namespace adis16448
{
ADIS16448 *g_dev{nullptr};
ADIS16448 *g_dev;
int info();
int start(enum Rotation rotation);
int stop();
void usage();
/**
* Start the driver.
*/
int
start(enum Rotation rotation)
static int start(enum Rotation rotation)
{
if (g_dev != nullptr) {
// If already started, the still command succeeded.
PX4_INFO("already started");
PX4_WARN("already started");
return 0;
}
// Create the driver.
// create the driver
#if defined(PX4_SPI_BUS_EXT)
g_dev = new ADIS16448(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation);
#elif defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16448(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation);
#else
PX4_ERR("External SPI not available");
return -1;
#endif
if (g_dev != nullptr) {
if (g_dev->init() == OK) {
return PX4_OK;
}
delete g_dev;
g_dev = nullptr;
if (g_dev == nullptr) {
PX4_ERR("driver start failed");
return -1;
}
PX4_ERR("driver start failed");
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
return PX4_ERROR;
return 0;
}
int stop()
static int stop()
{
if (g_dev == nullptr) {
PX4_INFO("driver not running");
return PX4_ERROR;
PX4_WARN("driver not running");
return -1;
}
delete g_dev;
g_dev = nullptr;
return PX4_OK;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
static int status()
{
if (g_dev == nullptr) {
PX4_INFO("driver not running");
return 0;
}
g_dev->print_info();
return PX4_OK;
return 0;
}
void
usage()
static int usage()
{
PX4_INFO("missing command: try 'start', 'info', 'stop'");
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
return 0;
}
} // namespace
} // namespace adis16448
/**
* Driver 'main' command.
*/
extern "C" int adis16448_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int ch;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
/* start options */
while ((ch = getopt(argc, argv, "R:")) != EOF) {
// start options
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(optarg);
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
adis16448::usage();
exit(0);
return adis16448::usage();
}
}
const char *verb = argv[optind];
const char *verb = argv[myoptind];
// Start/load the driver.
if (!strcmp(verb, "start")) {
return adis16448::start(rotation);
}
// Print driver information.
if (!strcmp(verb, "info")) {
return adis16448::info();
}
// Stop
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return adis16448::stop();
} else if (!strcmp(verb, "status")) {
return adis16448::status();
}
adis16448::usage();
return PX4_OK;
return adis16448::usage();
}

109
src/drivers/imu/adis16477/adis16477_main.cpp
View File

@ -33,91 +33,90 @@
#include "ADIS16477.hpp"
extern "C" { __EXPORT int adis16477_main(int argc, char *argv[]); }
#include <px4_platform_common/getopt.h>
/**
* Local functions in support of the shell command.
*/
namespace adis16477
{
ADIS16477 *g_dev{nullptr};
void start(enum Rotation rotation);
void info();
void usage();
/**
* Start the driver.
*/
void
start(enum Rotation rotation)
static int start(enum Rotation rotation)
{
if (g_dev != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
}
/* create the driver */
// create the driver
#if defined(PX4_SPIDEV_ADIS16477)
g_dev = new ADIS16477(PX4_SPI_BUS_SENSOR1, PX4_SPIDEV_ADIS16477, rotation);
#elif defined(PX4_SPI_BUS_EXT)
g_dev = new ADIS16477(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation);
#elif defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16477(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
return -1;
#endif
if (g_dev == nullptr) {
goto fail;
PX4_ERR("driver start failed");
return -1;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
PX4_ERR("driver start failed");
return 0;
}
/**
* Print a little info about the driver.
*/
void
info()
static int stop()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_INFO("driver not running");
return 0;
}
g_dev->print_info();
return 0;
}
void
usage()
static int usage()
{
PX4_INFO("missing command: try 'start', 'info'");
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
return 0;
}
}
// namespace
} // namespace adis16477
int
adis16477_main(int argc, char *argv[])
extern "C" int adis16477_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
/* start options */
// start options
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
@ -125,29 +124,21 @@ adis16477_main(int argc, char *argv[])
break;
default:
adis16477::usage();
return 0;
return adis16477::usage();
}
}
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
adis16477::start(rotation);
return adis16477::start(rotation);
} else if (!strcmp(verb, "stop")) {
return adis16477::stop();
} else if (!strcmp(verb, "status")) {
return adis16477::status();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
adis16477::info();
}
adis16477::usage();
return 0;
return adis16477::usage();
}

105
src/drivers/imu/adis16497/adis16497_main.cpp
View File

@ -33,91 +33,86 @@
#include "ADIS16497.hpp"
extern "C" { __EXPORT int adis16497_main(int argc, char *argv[]); }
#include <px4_platform_common/getopt.h>
/**
* Local functions in support of the shell command.
*/
namespace adis16497
{
ADIS16497 *g_dev{nullptr};
void start(enum Rotation rotation);
void info();
void usage();
/**
* Start the driver.
*/
void
start(enum Rotation rotation)
static int start(enum Rotation rotation)
{
if (g_dev != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
}
/* create the driver */
// create the driver
#if defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16497(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
return -1;
#endif
if (g_dev == nullptr) {
goto fail;
PX4_ERR("driver start failed");
return -1;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
PX4_ERR("driver start failed");
return 0;
}
/**
* Print a little info about the driver.
*/
void
info()
static int stop()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_INFO("driver not running");
return 0;
}
g_dev->print_info();
return 0;
}
void
usage()
static int usage()
{
PX4_INFO("missing command: try 'start', 'info'");
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
return 0;
}
}
// namespace
} // namespace adis16497
int
adis16497_main(int argc, char *argv[])
extern "C" int adis16497_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
/* start options */
// start options
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
@ -125,29 +120,21 @@ adis16497_main(int argc, char *argv[])
break;
default:
adis16497::usage();
return 0;
return adis16497::usage();
}
}
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
adis16497::start(rotation);
return adis16497::start(rotation);
} else if (!strcmp(verb, "stop")) {
return adis16497::stop();
} else if (!strcmp(verb, "status")) {
return adis16497::status();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
adis16497::info();
}
adis16497::usage();
return 0;
return adis16497::usage();
}