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

1 files changed, 64 insertions, 1 deletions

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,11 +119,18 @@ 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.
 	 * 
 	 * Handle all IOCTL calls to the PX4IO file descriptor.
@@ -476,6 +483,29 @@ PX4IO::~PX4IO()
 }
 
 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()
 {
 	int ret;
@@ -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");
 	}
 
@@ -1921,6 +1952,35 @@ start(int argc, char *argv[])
 }
 
 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[])
 {
 	int pulses;
@@ -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) {