More implementation for the serial side on IO; fix a couple of bugs on the FMU side.

Still needs serial init and some more testing/config on the FMU side, but closer to being ready to test.

6 files changed, 108 insertions, 80 deletions

diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h
index c4c592fe4..282ed7548 100755
--- a/src/drivers/boards/px4iov2/px4iov2_internal.h
+++ b/src/drivers/boards/px4iov2/px4iov2_internal.h
@@ -57,6 +57,12 @@
 /* Configuration **************************************************************/
 
 /******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define SERIAL_BASE	STM32_USART1_BASE
+#define SERIAL_VECTOR	STM32_IRQ_USART1
+
+/******************************************************************************
  * GPIOS
  ******************************************************************************/
 
diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp
index f91284c72..d0af2912a 100644
--- a/src/drivers/px4io/interface_serial.cpp
+++ b/src/drivers/px4io/interface_serial.cpp
@@ -160,6 +160,8 @@ PX4IO_serial::PX4IO_serial(int port) :
 		return;
 	}
 
+	/* XXX need to configure the port here */
+
 	/* need space for worst-case escapes + hx protocol overhead */
 	/* XXX this is kinda gross, but hx transmits a byte at a time */
 	_tx_buf = new uint8_t[HX_STREAM_MAX_FRAME];
@@ -257,7 +259,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
 		return -EIO;
 	} else {
 		/* copy back the result */
-		memcpy(values, &_tx_buf[0], count);
+		memcpy(values, &_rx_buf[0], count);
 	}
 out:
 	sem_post(&_bus_semaphore);
@@ -267,16 +269,12 @@ out:
 int
 PX4IO_serial::_wait_complete()
 {
-	/* prepare the stream for transmission */
+	/* prepare the stream for transmission (also discards any received noise) */
 	hx_stream_reset(_stream);
 	hx_stream_start(_stream, _tx_buf, _tx_size);
 
-	/* enable UART */
-	_CR1() |= USART_CR1_RE |
-		  USART_CR1_TE |
-		  USART_CR1_TXEIE |
-		  USART_CR1_RXNEIE |
-		  USART_CR1_UE;
+	/* enable transmit-ready interrupt, which will start transmission */
+	_CR1() |= USART_CR1_TXEIE;
 
 	/* compute the deadline for a 5ms timeout */
 	struct timespec abstime;
@@ -290,13 +288,6 @@ PX4IO_serial::_wait_complete()
 	/* wait for the transaction to complete */
 	int ret = sem_timedwait(&_completion_semaphore, &abstime);
 
-	/* disable the UART */
-	_CR1() &= ~(USART_CR1_RE |
-		    USART_CR1_TE |
-		    USART_CR1_TXEIE |
-		    USART_CR1_RXNEIE |
-		    USART_CR1_UE);
-
 	return ret;
 }
 
@@ -317,7 +308,7 @@ PX4IO_serial::_do_interrupt()
 	if (sr & USART_SR_TXE) {
 		int c = hx_stream_send_next(_stream);
 		if (c == -1) {
-			/* transmit (nearly) done, not interested in TX-ready interrupts now */
+			/* no more bytes to send, not interested in interrupts now */
 			_CR1() &= ~USART_CR1_TXEIE;
 
 			/* was this a tx-only operation? */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 2077e6244..47bcb8ddf 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,7 +42,12 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <drivers/boards/px4io/px4io_internal.h>
+#ifdef CONFIG_ARCH_BOARD_PX4IO
+# include <drivers/boards/px4io/px4io_internal.h>
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4IOV2
+# include <drivers/boards/px4iov2/px4iov2_internal.h>
+#endif
 
 #include "protocol.h"
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index df7d6dcd3..42554456c 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -44,6 +44,7 @@
 #include <string.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -349,10 +350,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
+#ifdef POWER_RELAY1
 			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+#endif
+#ifdef POWER_RELAY2
 			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+#endif
+#ifdef POWER_ACC1
 			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+#endif
+#ifdef POWER_ACC2
 			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+#endif
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index bf9456e94..f691969c4 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -46,36 +46,40 @@
 #include <nuttx/arch.h>
 #include <arch/board/board.h>
 
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32_internal.h>
 #include <systemlib/hx_stream.h>
 
 //#define DEBUG
 #include "px4io.h"
 
-static uint8_t		tx_buf[66];		/* XXX hardcoded magic number */
-
 static hx_stream_t	if_stream;
+static volatile bool	sending = false;
 
+static int		serial_interrupt(int vector, void *context);
 static void		serial_callback(void *arg, const void *data, unsigned length);
 
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
 void
 interface_init(void)
 {
 
-	int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK);
-	if (fd < 0) {
-		debug("serial fail");
-		return;
-	}
-
-	/* configure serial port - XXX increase port speed? */
-	struct termios t;
-	tcgetattr(fd, &t);
-	cfsetspeed(&t, 115200);
-	t.c_cflag &= ~(CSTOPB | PARENB);
-	tcsetattr(fd, TCSANOW, &t);
+	/* XXX do serial port init here */
 
-	/* allocate the HX stream we'll use for communication */
-	if_stream = hx_stream_init(fd, serial_callback, NULL);
+	irq_attach(SERIAL_VECTOR, serial_interrupt);
+	if_stream = hx_stream_init(-1, serial_callback, NULL);
 
 	/* XXX add stream stats counters? */
 
@@ -85,8 +89,31 @@ interface_init(void)
 void
 interface_tick()
 {
-	/* process incoming bytes */
-	hx_stream_rx(if_stream);
+	/* XXX nothing interesting to do here */
+}
+
+static int
+serial_interrupt(int vector, void *context)
+{
+	uint32_t sr = rSR;
+
+	if (sr & USART_SR_TXE) {
+		int c = hx_stream_send_next(if_stream);
+		if (c == -1) {
+			/* no more bytes to send, not interested in interrupts now */
+			rCR1 &= ~USART_CR1_TXEIE;
+			sending = false;
+		} else {
+			rDR = c;
+		}
+	}
+
+	if (sr & USART_SR_RXNE) {
+		uint8_t c = rDR;
+
+		hx_stream_rx(if_stream, c);
+	}
+	return 0;
 }
 
 static void
@@ -98,36 +125,40 @@ serial_callback(void *arg, const void *data, unsigned length)
 	if (length < 2)
 		return;
 
-	/* it's a write operation, pass it to the register API */
-	if (message[0] & PX4IO_PAGE_WRITE) {
-		registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
-			(const uint16_t *)&message[2],
-			(length - 2) / 2);
-
+	/* we got a new request while we were still sending the last reply - not supported */
+	if (sending)
 		return;
-	}
 
-	/* it's a read - must contain length byte */
-	if (length != 3)
+	/* reads are page / offset / length */
+	if (length == 3) {
+		uint16_t *registers;
+		unsigned count;
+
+		/* get registers for response, send an empty reply on error */
+		if (registers_get(message[0], message[1], &registers, &count) < 0)
+			count = 0;
+
+		/* constrain count to requested size or message limit */
+		if (count > message[2])
+			count = message[2];
+		if (count > HX_STREAM_MAX_FRAME)
+			count = HX_STREAM_MAX_FRAME;
+
+		/* start sending the reply */
+		sending = true;
+		hx_stream_reset(if_stream);
+		hx_stream_start(if_stream, registers, count * 2 + 2);
+
+		/* enable the TX-ready interrupt */
+		rCR1 |= USART_CR1_TXEIE;
 		return;
-	uint16_t *registers;
-	unsigned count;
-
-	tx_buf[0] = message[0];
-	tx_buf[1] = message[1];
-
-	/* get registers for response, send an empty reply on error */
-	if (registers_get(message[0], message[1], &registers, &count) < 0)
-		count = 0;
-
-	/* fill buffer with message, limited by length */
-#define TX_MAX		((sizeof(tx_buf) - 2) / 2)
-	if (count > TX_MAX)
-		count = TX_MAX;
-	if (count > message[2])
-		count = message[2];
-	memcpy(&tx_buf[2], registers, count * 2);
-
-	/* try to send the message */
-	hx_stream_send(if_stream, tx_buf, count * 2 + 2);
+
+	} else {
+
+		/* it's a write operation, pass it to the register API */
+		registers_set(message[0], 
+				message[1],
+				(const uint16_t *)&message[2],
+				(length - 2) / 2);
+	}
 }
\ No newline at end of file
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index fdc3edac7..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -96,20 +96,6 @@ hx_tx_raw(hx_stream_t stream, uint8_t c)
 		stream->tx_error = true;
 }
 
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
-	switch (c) {
-	case FBO:
-	case CEO:
-		hx_tx_raw(stream, CEO);
-		c ^= 0x20;
-		break;
-	}
-
-	hx_tx_raw(stream, c);
-}
-
 static int
 hx_rx_frame(hx_stream_t stream)
 {
@@ -281,12 +267,12 @@ hx_stream_send(hx_stream_t stream,
 {
 	int result;
 
-	result = hx_start(stream, data, count);
+	result = hx_stream_start(stream, data, count);
 	if (result != OK)
 		return result;
 
 	int c;
-	while ((c = hx_send_next(stream)) >= 0)
+	while ((c = hx_stream_send_next(stream)) >= 0)
 		hx_tx_raw(stream, c);
 
 	/* check for transmit error */