1 files changed, 373 insertions, 0 deletions
diff --git a/nuttx/configs/px4io/src/drv_ppm_input.c b/nuttx/configs/px4io/src/drv_ppm_input.c
new file mode 100644
index 000000000..f25ae41e2
--- /dev/null
+++ b/nuttx/configs/px4io/src/drv_ppm_input.c
@@ -0,0 +1,373 @@
+/****************************************************************************
+ *
+ *   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 input decoder.
+ *
+ * Works in conjunction with the HRT driver.
+ */
+
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include <arch/board/board.h>
+#include <arch/board/drv_ppm_input.h>
+#include <arch/board/up_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_HRT_PPM
+# ifndef CONFIG_HRT_TIMER
+#  error CONFIG_HRT_PPM requires CONFIG_HRT_TIMER
+# endif
+
+/*
+ * 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	750		/* shortest valid channel signal */
+#define PPM_MAX_CHANNEL_VALUE	2400		/* longest valid channel signal */
+#define PPM_MIN_START		5000		/* 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
+static uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+static unsigned ppm_decoded_channels;
+
+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;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+/* last time we got good data */
+static hrt_abstime ppm_timestamp;
+
+#ifndef CONFIG_DISABLE_MQUEUE
+/* message queue we advertise PPM data on */
+static mqd_t ppm_message_queue;
+#endif
+
+/* set if PPM data has not been read */
+static bool ppm_fresh_data;
+
+/* PPM device node file ops */
+
+static int ppm_read(struct file *filp, char *buffer, size_t len);
+static int ppm_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+static const struct file_operations ppm_fops = {
+	.read = ppm_read,
+	.ioctl = ppm_ioctl
+};
+
+/*
+ * Initialise the PPM system for client use.
+ */
+int
+ppm_input_init(const char *mq_name)
+{
+	int	err;
+
+	/* configure the PPM input pin */
+	stm32_configgpio(GPIO_PPM_IN);
+
+	/* and register the device node */
+	if (OK != (err = register_driver(PPM_DEVICE_NODE, &ppm_fops, 0666, NULL)))
+		return err;
+
+#ifndef CONFIG_DISABLE_MQUEUE
+	if (mq_name != NULL) {
+		/* create the message queue */
+		struct mq_attr attr = {
+			.mq_maxmsg = 1,
+			.mq_msgsize = sizeof(ppm_buffer)
+		};
+		ppm_message_queue = mq_open(mq_name, O_WRONLY | O_CREAT | O_NONBLOCK, 0666, &attr);
+		if (ppm_message_queue < 0)
+			return -errno;
+	}
+#endif
+
+	return OK;
+}
+
+/*
+ * Handle the PPM decoder state machine.
+ */
+void
+ppm_input_decode(bool reset, uint16_t 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;
+	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 int new_channel_count;
+			static int 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_timestamp = hrt_absolute_time();
+				ppm_fresh_data = true;
+#ifndef CONFIG_DISABLE_MQUEUE
+				/* advertise the new data to the message queue */
+				mq_send(ppm_message_queue, ppm_buffer, ppm_decoded_channels * sizeof(ppm_buffer[0]), 0);
+#endif
+			}
+		}
+
+		/* 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;
+}
+
+static int
+ppm_read(struct file *filp, char *buffer, size_t len)
+{
+	size_t avail;
+
+	/* the size of the returned data indicates the number of channels */
+	avail = ppm_decoded_channels * sizeof(ppm_buffer[0]);
+
+	/* if we have not decoded a frame, that's an I/O error */
+	if (avail == 0)
+		return -EIO;
+
+	/* if the caller's buffer is too small, that's also bad */
+	if (len < avail)
+		return -EFBIG;
+
+	/* if the caller doesn't want to block, and there is no fresh data, that's EWOULDBLOCK */
+	if ((filp->f_oflags & O_NONBLOCK) && (!ppm_fresh_data))
+		return -EWOULDBLOCK;
+
+	/*
+	 * Return the channel data.
+	 *
+	 * Note that we have to block the HRT while copying to avoid the
+	 * possibility that we'll get interrupted in the middle of copying 
+	 * a single value.
+	 */
+	irqstate_t flags = irqsave();
+
+	memcpy(buffer, ppm_buffer, avail);
+	ppm_fresh_data = false;
+
+	irqrestore(flags);
+
+	return OK;
+}
+
+static int
+ppm_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case PPM_INPUT_STATUS:
+		/* if we have received a frame within the timeout, the signal is "good" */
+		if ((hrt_absolute_time() - ppm_timestamp) < PPM_INPUT_TIMEOUT) {
+			*(ppm_input_status_t *)arg = PPM_STATUS_SIGNAL_CURRENT;
+		} else {
+			/* reset the number of channels so that any attempt to read data will fail */
+			ppm_decoded_channels = 0;
+			*(ppm_input_status_t *)arg = PPM_STATUS_NO_SIGNAL;
+		}
+		return OK;
+
+	case PPM_INPUT_CHANNELS:
+		*(ppm_input_channel_count_t *)arg = ppm_decoded_channels;
+		return OK;
+
+	default:
+		return -ENOTTY;
+	}
+
+}
+
+#endif /* CONFIG_HRT_PPM */
+
+