Add a test hook for the PX4IO interface. Wire up some simple tests for the serial interface.

Signal quality looks good at 1.5Mbps. Transmit timing is ~450µs per packet, ~20µs packet-to-packet delay spinning in a loop transmitting.

1 files changed, 135 insertions, 117 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ecf50c859..663609aed 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -104,8 +104,8 @@ public:
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate    The rate in Hz actuator outpus are sent to IO.
-	*      Min 10 Hz, max 400 Hz
+	* @param rate		The rate in Hz actuator outpus are sent to IO.
+	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
 
@@ -120,14 +120,14 @@ public:
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len Number of channels, could up to 8
+	 * @param vals		Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param len		Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
-	* Print the current status of IO
-	*/
+	 * Print the current status of IO
+	 */
 	void			print_status();
 
 private:
@@ -1579,7 +1579,7 @@ start(int argc, char *argv[])
 	PX4IO_interface *interface;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
-	interface = io_serial_interface(5);	/* XXX wrong port! */
+	interface = io_serial_interface();
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU)
 	interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
 #else
@@ -1710,6 +1710,35 @@ monitor(void)
 	}
 }
 
+void
+if_test(unsigned mode)
+{
+	PX4IO_interface *interface;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
+	interface = io_serial_interface();
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU)
+	interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+#else
+# error Unknown board - cannot select interface.
+#endif
+
+	if (interface == nullptr)
+		errx(1, "cannot alloc interface");
+
+	if (!interface->ok()) {
+		delete interface;
+		errx(1, "interface init failed");
+	} else {
+
+		int result = interface->test(mode);
+		delete interface;
+		errx(0, "test returned %d", result);
+	}
+
+	exit(0);
+}
+
 }
 
 int
@@ -1722,28 +1751,85 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
+	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+		}
 
-			if ((argc > 2)) {
-				g_dev->set_update_rate(atoi(argv[2]));
-			} else {
-				errx(1, "missing argument (50 - 200 Hz)");
-				return 1;
-			}
+		PX4IO_Uploader *up;
+		const char *fn[3];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+
+		return ret;
+	}
+
+	if (!strcmp(argv[1], "iftest")) {
+		if (g_dev != nullptr)
+			errx(1, "can't iftest when started");
+
+		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+	}
+
+	/* commands below here require a started driver */
+
+	if (g_dev == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "limit")) {
+
+		if ((argc > 2)) {
+			g_dev->set_update_rate(atoi(argv[2]));
+		} else {
+			errx(1, "missing argument (50 - 200 Hz)");
+			return 1;
 		}
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "current")) {
-		if (g_dev != nullptr) {
-			if ((argc > 3)) {
-				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
-			} else {
-				errx(1, "missing argument (apm_per_volt, amp_offset)");
-				return 1;
-			}
+		if ((argc > 3)) {
+			g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+		} else {
+			errx(1, "missing argument (apm_per_volt, amp_offset)");
+			return 1;
 		}
 		exit(0);
 	}
@@ -1754,70 +1840,54 @@ px4io_main(int argc, char *argv[])
 			errx(1, "failsafe command needs at least one channel value (ppm)");
 		}
 
-		if (g_dev != nullptr) {
+		/* set values for first 8 channels, fill unassigned channels with 1500. */
+		uint16_t failsafe[8];
 
-			/* set values for first 8 channels, fill unassigned channels with 1500. */
-			uint16_t failsafe[8];
-
-			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
-			{
-				/* set channel to commanline argument or to 900 for non-provided channels */
-				if (argc > i + 2) {
-					failsafe[i] = atoi(argv[i+2]);
-					if (failsafe[i] < 800 || failsafe[i] > 2200) {
-						errx(1, "value out of range of 800 < value < 2200. Aborting.");
-					}
-				} else {
-					/* a zero value will result in stopping to output any pulse */
-					failsafe[i] = 0;
+		for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
+
+			/* set channel to commandline argument or to 900 for non-provided channels */
+			if (argc > i + 2) {
+				failsafe[i] = atoi(argv[i+2]);
+				if (failsafe[i] < 800 || failsafe[i] > 2200) {
+					errx(1, "value out of range of 800 < value < 2200. Aborting.");
 				}
+			} else {
+				/* a zero value will result in stopping to output any pulse */
+				failsafe[i] = 0;
 			}
+		}
 
-			int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+		int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
 
-			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
-		} else {
-			errx(1, "not loaded");
-		}
+		if (ret != OK)
+			errx(ret, "failed setting failsafe values");
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "recovery")) {
 
-		if (g_dev != nullptr) {
-			/*
-			 * Enable in-air restart support.
-			 * We can cheat and call the driver directly, as it
-		 	 * doesn't reference filp in ioctl()
-			 */
-			g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
-		} else {
-			errx(1, "not loaded");
-		}
+		/*
+		 * Enable in-air restart support.
+		 * We can cheat and call the driver directly, as it
+	 	 * doesn't reference filp in ioctl()
+		 */
+		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 
-		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
-		} else {
-			errx(1, "not loaded");
-		}
+		/* stop the driver */
+		delete g_dev;
 		exit(0);
 	}
 
 
 	if (!strcmp(argv[1], "status")) {
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded\n");
-			g_dev->print_status();
-		} else {
-			printf("[px4io] not loaded\n");
-		}
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
 
 		exit(0);
 	}
@@ -1844,58 +1914,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	/* note, stop not currently implemented */
-
-	if (!strcmp(argv[1], "update")) {
-
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded, detaching first\n");
-			/* stop the driver */
-			delete g_dev;
-		}
-
-		PX4IO_Uploader *up;
-		const char *fn[3];
-
-		/* work out what we're uploading... */
-		if (argc > 2) {
-			fn[0] = argv[2];
-			fn[1] = nullptr;
-
-		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] =	nullptr;
-		}
-
-		up = new PX4IO_Uploader;
-		int ret = up->upload(&fn[0]);
-		delete up;
-
-		switch (ret) {
-		case OK:
-			break;
-
-		case -ENOENT:
-			errx(1, "PX4IO firmware file not found");
-
-		case -EEXIST:
-		case -EIO:
-			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
-
-		case -EINVAL:
-			errx(1, "verify failed - retry the update");
-
-		case -ETIMEDOUT:
-			errx(1, "timed out waiting for bootloader - power-cycle and try again");
-
-		default:
-			errx(1, "unexpected error %d", ret);
-		}
-
-		return ret;
-	}
-
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -1909,6 +1927,6 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 
-	out:
+out:
 	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
 }