px4_autopilot/apps/px4io/dsm.c

/****************************************************************************
 *
 *   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();
}