Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs.

1 files changed, 184 insertions, 0 deletions

diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
new file mode 100644
index 000000000..79b6301c7
--- /dev/null
+++ b/apps/px4io/dsm.c
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 dsm.c
+ *
+ * Serial protocol decoder for the Spektrum DSM* family of protocols.
+ *
+ * Decodes into the global PPM buffer and updates accordingly.
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <systemlib/ppm_decode.h>
+ 
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+#include "protocol.h"
+
+#define DSM_FRAME_SIZE		16
+#define DSM_FRAME_CHANNELS	7
+
+static hrt_abstime last_frame_time;
+
+static uint8_t	frame[DSM_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+static bool	insync;
+static unsigned	channel_shift;
+
+static void dsm_decode(void);
+
+void
+dsm_init(unsigned mode)
+{
+	insync = false;
+	partial_frame_count = 0;
+
+	if (mode == RX_MODE_DSM_10BIT) {
+		channel_shift = 10;
+	} else {
+		channel_shift = 11;
+	}
+
+	last_frame_time = hrt_absolute_time();
+}
+
+void
+dsm_input(int fd)
+{
+	uint8_t		buf[DSM_FRAME_SIZE];
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 * The DSM* protocol doesn't provide any explicit framing,
+	 * so we detect frame boundaries by the inter-frame delay.
+	 *
+	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
+	 * frame transmission time is ~1.4ms.
+	 *
+	 * We expect to only be called when bytes arrive for processing,
+	 * and if an interval of more than 5ms passes between calls, 
+	 * the first byte we read will be the first byte of a frame.
+	 */
+	now = hrt_absolute_time();
+	if ((now - last_frame_time) > 5000)
+		partial_frame_count = 0;
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current frame.
+	 */
+	ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return;
+
+	/*
+	 * Add bytes to the current frame
+	 */
+	memcpy(&frame[partial_frame_count], buf, ret);
+	partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full frame, return
+	 */
+	if (partial_frame_count < DSM_FRAME_SIZE)
+	 	return;
+	last_frame_time = now;
+
+	/*
+	 * Great, it looks like we might have a frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_decode();
+	partial_frame_count = 0;
+}
+
+static void
+dsm_decode(void)
+{
+	uint16_t	data_mask = (1 << channel_shift) - 1;
+
+	/*
+	 * The encoding of the first byte is uncertain, so we're going
+	 * to ignore it for now.
+	 *
+	 * The second byte may tell us about the protocol, but it's not
+	 * actually very interesting since what we really want to know
+	 * is how the channel data is formatted, and there doesn't seem
+	 * to be a reliable way to determine this from the protocol ID
+	 * alone.
+	 *
+	 * Each channel is a 16-bit unsigned value containing either a 10-
+	 * or 11-bit channel value and a 4-bit channel number, shifted
+	 * either 10 or 11 bits. The MSB may also be set to indicate the
+	 * second frame in variants of the protocol where more than
+	 * seven channels are being transmitted.
+	 */
+
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+
+		/* ignore pad channels */
+		if (raw == 0xffff)
+			continue;
+
+		unsigned channel = (raw >> channel_shift) & 0xf;
+
+		/* ignore channels out of range */
+		if (channel >= PX4IO_INPUT_CHANNELS)
+			continue;
+
+		if (channel > ppm_decoded_channels)
+			ppm_decoded_channels = channel;
+
+		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		unsigned data = raw & data_mask;
+		if (channel_shift == 11)
+			data /= 2;
+		ppm_buffer[channel] = 988 + data;
+
+	}
+	ppm_last_valid_decode = hrt_absolute_time();
+}