Merge branch 'master' of github.com:PX4/Firmware into vector_control

4 files changed, 211 insertions, 71 deletions

diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
-
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
+
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -81,21 +93,7 @@ static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = 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);
+		stream->tx_error = true;
 }
 
 static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
 int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
-
-	if (resid > HX_STREAM_MAX_FRAME)
+	if (count > HX_STREAM_MAX_FRAME)
 		return -EINVAL;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	int result;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	int c;
+	while ((c = hx_stream_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
 void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
 		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_errors);
 
 /**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
+/**
  * Send a frame.
  *
  * This function will block until all frame bytes are sent if
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..3c1e10287 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
 	switch (handle->type) {
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			pce->event_count++;
-			pce->time_total += elapsed;
+			if (pce->time_start != 0) {
+				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			if ((pce->time_least > elapsed) || (pce->time_least == 0))
-				pce->time_least = elapsed;
+				pce->event_count++;
+				pce->time_total += elapsed;
 
-			if (pce->time_most < elapsed)
-				pce->time_most = elapsed;
+				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+					pce->time_least = elapsed;
+
+				if (pce->time_most < elapsed)
+					pce->time_most = elapsed;
+
+				pce->time_start = 0;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
 		}
+		break;
 
 	default:
 		break;
 	}
 }
 
+
+
 void
 perf_reset(perf_counter_t handle)
 {
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..4cd8b67a1 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -92,13 +92,25 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
  *
  * This call resets performance counter to initial state
  *