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.

src/drivers/px4io/interface.h

@@ -72,7 +72,14 @@ public:
 	 * @return		Zero on success.
 	 */	
 	virtual int	get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0;
+
+	/**
+	 * Perform an interface test.
+	 *
+	 * @param mode		Optional test mode.
+	 */
+	virtual int	test(unsigned mode) = 0;
 };
 
 extern PX4IO_interface	*io_i2c_interface(int bus, uint8_t address);
-extern PX4IO_interface	*io_serial_interface(int port);
+extern PX4IO_interface	*io_serial_interface();

src/drivers/px4io/interface_i2c.cpp

@@ -69,6 +69,8 @@ public:
 
 	virtual bool	ok();
 
+	virtual int	test(unsigned mode);
+
 private:
 	static const unsigned	_retries = 2;
 
@@ -107,6 +109,12 @@ PX4IO_I2C::ok()
 	return true;
 }
 
+int
+PX4IO_I2C::test(unsigned mode)
+{
+	return 0;
+}
+
 int
 PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {

src/drivers/px4io/interface_serial.cpp

@@ -87,6 +87,7 @@ public:
 	virtual int	get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
 	virtual bool	ok();
+	virtual int	test(unsigned mode);
 
 private:
 	/*
@@ -102,10 +103,7 @@ private:
 	 */
 	static IOPacket		_dma_buffer;		// XXX static to ensure DMA-able memory
 
-	static const unsigned	_serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP;
 	DMA_HANDLE		_tx_dma;
-
-	static const unsigned	_serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP;
 	DMA_HANDLE		_rx_dma;
 
 	/** set if we have started a transaction that expects a reply */
@@ -164,7 +162,7 @@ private:
 
 IOPacket PX4IO_serial::_dma_buffer;
 
-PX4IO_interface	*io_serial_interface(int port)
+PX4IO_interface	*io_serial_interface()
 {
 	return new PX4IO_serial();
 }
@@ -175,8 +173,8 @@ PX4IO_serial::PX4IO_serial() :
 	_expect_reply(false)
 {
 	/* allocate DMA */
-	_tx_dma = stm32_dmachannel(_serial_tx_dma);
+	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
-	_rx_dma = stm32_dmachannel(_serial_rx_dma);
+	_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
 	if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
 		return;
 
@@ -209,14 +207,25 @@ PX4IO_serial::PX4IO_serial() :
 
 PX4IO_serial::~PX4IO_serial()
 {
-	if (_tx_dma != nullptr)
+	if (_tx_dma != nullptr) {
+		stm32_dmastop(_tx_dma);
 		stm32_dmafree(_tx_dma);
-	if (_rx_dma != nullptr)
+	}
+	if (_rx_dma != nullptr) {
+		stm32_dmastop(_rx_dma);
 		stm32_dmafree(_rx_dma);
+	}
 
+	/* reset the UART */
+	_CR1(0);
+	_CR2(0);
+	_CR3(0);
+
+	/* restore the GPIOs */
 	stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
 	stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
 
+	/* and kill our semaphores */
 	sem_destroy(&_completion_semaphore);
 	sem_destroy(&_bus_semaphore);
 }
@@ -232,6 +241,40 @@ PX4IO_serial::ok()
 	return true;
 }
 
+int
+PX4IO_serial::test(unsigned mode)
+{
+
+	switch (mode) {
+	case 0:
+		lowsyslog("test 0\n");
+
+		/* kill DMA, this is a PIO test */
+		stm32_dmastop(_tx_dma);
+		stm32_dmastop(_rx_dma);
+		_CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT));
+
+		for (;;) {
+			while (!(_SR() & USART_SR_TXE))
+				;
+			_DR(0x55);
+		}
+		return 0;
+
+	case 1:
+		lowsyslog("test 1\n");
+		{
+			uint16_t value = 0x5555;
+			for (;;) {
+				if (set_reg(0x55, 0x55, &value, 1) != OK)
+					return -2;
+			}
+			return 0;
+		}
+	}
+	return -1;
+}
+
 int
 PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
@@ -297,16 +340,20 @@ PX4IO_serial::_wait_complete(bool expect_reply)
 		stm32_dmastart(_rx_dma, _dma_callback, this, false);
 
 	/* start TX DMA - no callback if we also expect a reply */
-	stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false);
+	stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false);
 
 	/* compute the deadline for a 5ms timeout */
 	struct timespec abstime;
 	clock_gettime(CLOCK_REALTIME, &abstime);
+#if 1
+	abstime.tv_sec++;
+#else
 	abstime.tv_nsec += 5000000;	/* 5ms timeout */
 	while (abstime.tv_nsec > 1000000000) {
 		abstime.tv_sec++;
 		abstime.tv_nsec -= 1000000000;
 	}
+#endif
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
@@ -316,11 +363,13 @@ PX4IO_serial::_wait_complete(bool expect_reply)
 		if (ret == OK)
 			break;
 
-		if (ret == ETIMEDOUT) {
+		if (errno == ETIMEDOUT) {
+			lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma));
 			/* something has broken - clear out any partial DMA state and reconfigure */
 			_reset_dma();
 			break;
 		}
+		lowsyslog("unexpected ret %d/%d\n", ret, errno);
 	}
 
 	return ret;

src/drivers/px4io/px4io.cpp

@@ -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,130 +1751,6 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
-
-		if (g_dev != nullptr) {
-
-			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;
-			}
-		}
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "failsafe")) {
-
-		if (argc < 3) {
-			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];
-
-			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;
-				}
-			}
-
-			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");
-		}
-		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");
-		}
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-
-		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
-		} else {
-			errx(1, "not loaded");
-		}
-		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");
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "debug")) {
-		if (argc <= 2) {
-			printf("usage: px4io debug LEVEL\n");
-			exit(1);
-		}
-		if (g_dev == nullptr) {
-			printf("px4io is not started\n");
-			exit(1);
-		}
-		uint8_t level = atoi(argv[2]);
-		/* we can cheat and call the driver directly, as it
-		 * doesn't reference filp in ioctl()
-		 */
-		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
-		if (ret != 0) {
-			printf("SET_DEBUG failed - %d\n", ret);
-			exit(1);
-		}
-		printf("SET_DEBUG %u OK\n", (unsigned)level);
-		exit(0);
-	}
-
-	/* note, stop not currently implemented */
-
 	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
@@ -1896,6 +1801,119 @@ px4io_main(int argc, char *argv[])
 		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 ((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);
+	}
+
+	if (!strcmp(argv[1], "failsafe")) {
+
+		if (argc < 3) {
+			errx(1, "failsafe command needs at least one channel value (ppm)");
+		}
+
+		/* 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 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]));
+
+		if (ret != OK)
+			errx(ret, "failed setting failsafe values");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "recovery")) {
+
+		/*
+		 * 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")) {
+
+		/* stop the driver */
+		delete g_dev;
+		exit(0);
+	}
+
+
+	if (!strcmp(argv[1], "status")) {
+
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "debug")) {
+		if (argc <= 2) {
+			printf("usage: px4io debug LEVEL\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		uint8_t level = atoi(argv[2]);
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
+		if (ret != 0) {
+			printf("SET_DEBUG failed - %d\n", ret);
+			exit(1);
+		}
+		printf("SET_DEBUG %u OK\n", (unsigned)level);
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||