1 files changed, 248 insertions, 0 deletions
diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c
new file mode 100644
index 000000000..a5d2f738d
--- /dev/null
+++ b/src/modules/systemlib/ppm_decode.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   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 ppm_decode.c
+ *
+ * PPM input decoder.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "ppm_decode.h"
+
+/*
+ * PPM decoder tuning parameters.
+ *
+ * The PPM decoder works as follows.
+ *
+ * Initially, the decoder waits in the UNSYNCH state for two edges
+ * separated by PPM_MIN_START.  Once the second edge is detected,
+ * the decoder moves to the ARM state.
+ *
+ * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the
+ * timing mark for the first channel.  If this is detected, it moves to
+ * the INACTIVE state.
+ *
+ * The INACTIVE phase waits for and discards the next edge, as it is not
+ * significant.  Once the edge is detected, it moves to the ACTIVE stae.
+ *
+ * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
+ * received calculates the time from the previous mark and records
+ * this time as the value for the next channel.
+ *
+ * If at any time waiting for an edge, the delay from the previous edge
+ * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
+ * values are advertised to clients.
+ */
+#define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+#define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+#define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+#define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* Input timeout - after this interval we assume signal is lost */
+#define PPM_INPUT_TIMEOUT	100 * 1000	/* 100ms */
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	3		/* should be less than the input timeout */
+
+/* decoded PPM buffer */
+#define PPM_MIN_CHANNELS	4
+#define PPM_MAX_CHANNELS	12
+
+/*
+ * Public decoder state
+ */
+uint16_t	ppm_buffer[PPM_MAX_CHANNELS];
+unsigned	ppm_decoded_channels;
+hrt_abstime	ppm_last_valid_decode;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+static struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	unsigned	count_max;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+
+void
+ppm_input_init(unsigned count_max)
+{
+	ppm_decoded_channels = 0;
+	ppm_last_valid_decode = 0;
+
+	memset(&ppm, 0, sizeof(ppm));
+	ppm.count_max = count_max;
+}
+
+void
+ppm_input_decode(bool reset, unsigned count)
+{
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (reset)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+
+	if (count < ppm.last_edge)
+		width += ppm.count_max;	/* handle wrapped count */
+
+	ppm.last_edge = count;
+
+	/*
+	 * If this looks like a start pulse, then push the last set of values
+	 * and reset the state machine.
+	 *
+	 * Note that this is not a "high performance" design; it implies a whole
+	 * frame of latency between the pulses being received and their being
+	 * considered valid.
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+
+		/* note that we don't bother looking at the timing of this edge */
+
+		return;
+
+	case ACTIVE:
+
+		/* we expect a well-formed pulse */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+}
+