Serial interface for IOv2

8 files changed, 249 insertions, 45 deletions

diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..10aeb5c9f 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -69,6 +69,7 @@ static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+static void		i2c_dump(void);
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
@@ -92,7 +93,7 @@ enum {
 } direction;
 
 void
-i2c_init(void)
+interface_init(void)
 {
 	debug("i2c init");
 
@@ -148,12 +149,18 @@ i2c_init(void)
 #endif
 }
 
+void
+interface_tick()
+{
+}
+
 
 /*
   reset the I2C bus
   used to recover from lockups
  */
-void i2c_reset(void)
+void
+i2c_reset(void)
 {
 	rCR1 |= I2C_CR1_SWRST;
 	rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
-void
+static void
 i2c_dump(void)
 {
 	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 016be6d3b..4dd1aa8d7 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -8,7 +8,6 @@ SRCS		= adc.c \
 		  safety.c \
 		  sbus.c \
 		  ../systemlib/up_cxxinitialize.c \
-		  ../systemlib/hx_stream.c \
 		  ../systemlib/perf_counter.c \
 		  mixer.cpp \
 		  ../systemlib/mixer/mixer.cpp \
@@ -16,11 +15,10 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
 
-ifneq ($(CONFIG_ARCH_BOARD_PX4IO),)
+ifeq ($(BOARD),px4io)
 SRCS		+= i2c.c
-EXTRADEFINES	+= -DINTERFACE_I2C
 endif
-ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),)
-#SRCS		+= serial.c
-EXTRADEFINES	+= -DINTERFACE_SERIAL
+ifeq ($(BOARD),px4iov2)
+SRCS		+= serial.c \
+		   ../systemlib/hx_stream.c
 endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 8d8b7e941..1a687bd2a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,7 @@
 /**
  * @file protocol.h
  *
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -62,6 +62,27 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
+ *
+ * PX4IO I2C interface notes:
+ *
+ * Register read/write operations are mapped directly to PX4IO register
+ * read/write operations.
+ *
+ * PX4IO Serial interface notes:
+ *
+ * The MSB of the page number is used to distinguish between read and
+ * write operations. If set, the operation is a write and additional 
+ * data is expected to follow in the packet as for I2C writes.
+ *
+ * If clear, the packet is expected to contain a single byte giving the
+ * number of bytes to be read. PX4IO will respond with a packet containing
+ * the same header (page, offset) and the requested data.
+ *
+ * If a read is requested when PX4IO does not have buffer space to store
+ * the reply, the request will be dropped. PX4IO is always configured with
+ * enough space to receive one full-sized write and one read request, and
+ * to send one full-sized read response.
+ *
  */
 
 #define PX4IO_CONTROL_CHANNELS			8
@@ -75,12 +96,14 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_PAGE_WRITE		(1<<7)
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
-#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum packet transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
@@ -141,7 +164,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP			100
+#define PX4IO_PAGE_SETUP		64
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -160,13 +183,13 @@
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS		65		/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD			102
+#define PX4IO_PAGE_MIXERLOAD		66		/* see px4io_mixdata structure below */
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG		67		/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -178,10 +201,10 @@
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM		68		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM		69		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 39f05c112..385920d53 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -142,7 +142,7 @@ user_start(int argc, char *argv[])
 	LED_BLUE(false);
 	LED_SAFETY(false);
 
-	/* turn on servo power */
+	/* turn on servo power (if supported) */
 	POWER_SERVO(true);
 
 	/* start the safety switch handler */
@@ -154,13 +154,11 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
-#ifdef INTERFACE_I2C
-	/* start the i2c handler */
-	i2c_init();
-#endif
-#ifdef INTERFACE_SERIAL
-	/* start the serial interface */
-#endif
+	/* start the FMU interface */
+	interface_init();
+
+	/* add a performance counter for the interface */
+	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
 
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -203,6 +201,11 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
+		/* kick the interface */
+		perf_begin(interface_perf);
+		interface_tick();
+		perf_end(interface_perf);
+
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..2077e6244 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -125,16 +125,16 @@ extern struct sys_state_s system_state;
 
 #define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
 #ifdef GPIO_ACC1_PWR_EN
-    #define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
 #endif
 #ifdef GPIO_ACC2_PWR_EN
-    #define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
 #endif
 #ifdef GPIO_RELAY1_EN
-    #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #endif
 #ifdef GPIO_RELAY2_EN
-    #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+# define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
 #endif
 
 #define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
@@ -156,12 +156,11 @@ extern void	mixer_handle_text(const void *buffer, size_t length);
  */
 extern void	safety_init(void);
 
-#ifdef CONFIG_STM32_I2C1
 /**
  * FMU communications
  */
-extern void	i2c_init(void);
-#endif
+extern void	interface_init(void);
+extern void	interface_tick(void);
 
 /**
  * Register space
@@ -190,10 +189,5 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
 
-/* send a debug message to the console */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
+/** send a debug message to the console */
+extern void	isr_debug(uint8_t level, const char *fmt, ...);
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..a12d58aca
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.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 void		serial_callback(void *arg, const void *data, unsigned length);
+
+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);
+
+	/* allocate the HX stream we'll use for communication */
+	if_stream = hx_stream_init(fd, serial_callback, NULL);
+
+	/* XXX add stream stats counters? */
+
+	debug("serial init");
+}
+
+void
+interface_tick()
+{
+	/* process incoming bytes */
+	hx_stream_rx(if_stream);
+}
+
+static void
+serial_callback(void *arg, const void *data, unsigned length)
+{
+	const uint8_t *message = (const uint8_t *)data;
+
+	/* malformed frame, ignore it */
+	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);
+
+		return;
+	}
+
+	/* it's a read */
+	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 */
+#define TX_MAX		((sizeof(tx_buf) - 2) / 2)
+	if (count > TX_MAX)
+		count = TX_MAX;
+	memcpy(&tx_buf[2], registers, count * 2);
+
+	/* try to send the message */
+	hx_stream_send(if_stream, tx_buf, count * 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 8d77f14a8..88f7f762c 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -76,6 +76,7 @@ struct hx_stream {
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static int	hx_rx_frame(hx_stream_t stream);
+static bool	hx_rx_char(hx_stream_t stream, uint8_t c);
 
 static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
@@ -224,13 +225,13 @@ hx_stream_send(hx_stream_t stream,
 	return 0;
 }
 
-void
-hx_stream_rx(hx_stream_t stream, uint8_t c)
+static bool
+hx_rx_char(hx_stream_t stream, uint8_t c)
 {
 	/* frame end? */
 	if (c == FBO) {
 		hx_rx_frame(stream);
-		return;
+		return true;
 	}
 
 	/* escaped? */
@@ -241,10 +242,43 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	} else if (c == CEO) {
 		/* now escaped, ignore the byte */
 		stream->escaped = true;
-		return;
+		return false;
 	}
 
 	/* save for later */
 	if (stream->frame_bytes < sizeof(stream->buf))
 		stream->buf[stream->frame_bytes++] = c;
+
+	return false;
+}
+
+void
+hx_stream_rx_char(hx_stream_t stream, uint8_t c)
+{
+	hx_rx_char(stream, c);
+}
+
+int
+hx_stream_rx(hx_stream_t stream)
+{
+	uint16_t	buf[16];
+	ssize_t		len;
+
+	/* read bytes */
+	len = read(stream->fd, buf, sizeof(buf));
+	if (len <= 0) {
+
+		/* nonblocking stream and no data */
+		if (errno == EWOULDBLOCK)
+			return 0;
+
+		/* error or EOF */
+		return -errno;
+	}
+
+	/* process received characters */
+	for (int i = 0; i < len; i++)
+		hx_rx_char(stream, buf[i]);
+
+	return 0;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..be4850f74 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -114,9 +114,25 @@ __EXPORT extern int		hx_stream_send(hx_stream_t stream,
  * @param stream	A handle returned from hx_stream_init.
  * @param c		The character to process.
  */
-__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
+__EXPORT extern void		hx_stream_rx_char(hx_stream_t stream,
 		uint8_t c);
 
+/**
+ * Handle received bytes from the stream.
+ *
+ * Note that this interface should only be used with blocking streams
+ * when it is OK for the call to block until a frame is received.
+ *
+ * When used with a non-blocking stream, it will typically return
+ * immediately, or after handling a received frame.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		-errno on error, nonzero if a frame
+ *			has been received, or if not enough
+ *			bytes are available to complete a frame.
+ */
+__EXPORT extern int		hx_stream_rx(hx_stream_t stream);
+
 __END_DECLS
 
 #endif