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

Added IO detect command to be smart about what to start before actually doing it

This commit is contained in:
Lorenz Meier 2013-08-28 14:58:53 +02:00
parent 49ef30b834
commit a48be0446b
1 changed files with 64 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
src/drivers/px4io/px4io.cpp
View File

@ -119,10 +119,17 @@ public:
/**
* Initialize the PX4IO class.
*
* Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*/
virtual int init();
/**
* Detect if a PX4IO is connected.
*
* Only validate if there is a PX4IO to talk to.
*/
virtual int detect();
/**
* IO Control handler.
*
@ -475,6 +482,29 @@ PX4IO::~PX4IO()
g_dev = nullptr;
}
int
PX4IO::detect()
{
int ret;
ASSERT(_task == -1);
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
log("protocol/firmware mismatch");
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
return -1;
}
return 0;
}
int
PX4IO::init()
{
@ -1894,6 +1924,7 @@ start(int argc, char *argv[])
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
@ -1920,6 +1951,35 @@ start(int argc, char *argv[])
exit(0);
}
void
detect(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->detect()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver detect did not succeed");
}
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
exit(0);
}
void
bind(int argc, char *argv[])
{
@ -2079,6 +2139,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
if (!strcmp(argv[1], "detect"))
detect(argc - 1, argv + 1);
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {