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.

4 files changed, 210 insertions, 128 deletions

diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h
index 834cb9e07..cb7da1081 100644
--- a/src/drivers/px4io/interface.h
+++ b/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();
diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp
index 6895a7e23..45a707883 100644
--- a/src/drivers/px4io/interface_i2c.cpp
+++ b/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;
 
@@ -108,6 +110,12 @@ PX4IO_I2C::ok()
 }
 
 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)
 {
 	int ret;
diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp
index 9471cecdd..817bcba8e 100644
--- a/src/drivers/px4io/interface_serial.cpp
+++ b/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);
-	_rx_dma = stm32_dmachannel(_serial_rx_dma);
+	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
+	_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);
 }
@@ -233,6 +242,40 @@ PX4IO_serial::ok()
 }
 
 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)
 {
 	if (num_values > max_rw_regs)
@@ -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;
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'");
 }