From a48be0446bf78f73d7550864150c1cf3de4dafa3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 28 Aug 2013 14:58:53 +0200
Subject: Added IO detect command to be smart about what to start before
 actually doing it

---
 src/drivers/px4io/px4io.cpp | 65 ++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 64 insertions(+), 1 deletion(-)

(limited to 'src/drivers/px4io/px4io.cpp')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 392bc9f0a..dda1b825c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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) {
-- 
cgit v1.2.3