Use common, board-type-agnostic code to allocate the PX4IO interface.

1 files changed, 32 insertions, 34 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 904da84c4..9f84c0950 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1598,30 +1598,43 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 namespace
 {
 
-void
-start(int argc, char *argv[])
+device::Device *
+get_interface()
 {
-	if (g_dev != nullptr)
-		errx(1, "already loaded");
+	device::Device *interface = nullptr;
 
-	device::Device *interface;
+	/* try for a serial interface */
+	if (PX4IO_serial_interface != nullptr)
+		interface = PX4IO_serial_interface();
+	if (interface != nullptr)
+		goto got;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-	interface = PX4IO_serial_interface();
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-	interface = PX4IO_i2c_interface();
-#else
-# error Unknown board - cannot select interface.
-#endif
+	/* try for an I2C interface if we haven't got a serial one */
+	if (PX4IO_i2c_interface != nullptr)
+		interface = PX4IO_i2c_interface();
+	if (interface != nullptr)
+		goto got;
 
-	if (interface == nullptr)
-		errx(1, "cannot alloc interface");
+	errx(1, "cannot alloc interface");
 
-	if (interface->init()) {
+got:
+	if (interface->init() != OK) {
 		delete interface;
 		errx(1, "interface init failed");
 	}
 
+	return interface;
+}
+
+void
+start(int argc, char *argv[])
+{
+	if (g_dev != nullptr)
+		errx(1, "already loaded");
+
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
 	/* create the driver - it will set g_dev */
 	(void)new PX4IO(interface);
 
@@ -1741,27 +1754,12 @@ monitor(void)
 void
 if_test(unsigned mode)
 {
-	device::Device *interface;
+	device::Device *interface = get_interface();
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-	interface = PX4IO_serial_interface();
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-	interface = PX4IO_i2c_interface();
-#else
-# error Unknown board - cannot select interface.
-#endif
+	int result = interface->ioctl(1, mode); /* XXX magic numbers */
+	delete interface;
 
-	if (interface == nullptr)
-		errx(1, "cannot alloc interface");
-
-	if (interface->init()) {
-		delete interface;
-		errx(1, "interface init failed");
-	} else {
-		int result = interface->ioctl(1, mode); /* XXX magic numbers */
-		delete interface;
-		errx(0, "test returned %d", result);
-	}
+	errx(0, "test returned %d", result);
 }
 
 } /* namespace */