px4-firmware/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>

#define DEBUG

#include "px4io.h"
#include "protocol.h"

#define DSM_FRAME_SIZE		16
#define DSM_FRAME_CHANNELS	7

static hrt_abstime last_rx_time;
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 bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
static void dsm_guess_format(bool reset);
static void dsm_decode(hrt_abstime now);

void
dsm_init(unsigned mode)
{
	insync = false;
	partial_frame_count = 0;
	last_rx_time = hrt_absolute_time();

	/* reset the format detector */
	dsm_guess_format(true);

	debug("DSM: enabled and waiting\n");
}

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.
	 *
	 * In the case where byte(s) are dropped from a frame, this also
	 * provides a degree of protection. Of course, it would be better
	 * if we didn't drop bytes...
	 */
	now = hrt_absolute_time();
	if ((now - last_rx_time) > 5000) {
		if (partial_frame_count > 0)
			debug("DSM: reset @ %d", partial_frame_count);
		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;
	last_rx_time = now;

	/*
	 * 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;

	/*
	 * Great, it looks like we might have a frame.  Go ahead and
	 * decode it.
	 */
	dsm_decode(now);
	partial_frame_count = 0;
}

static bool
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{

	if (raw == 0xffff)
		return false;

	*channel = (raw >> shift) & 0xf;

	uint16_t data_mask = (1 << shift) - 1;
	*value = raw & data_mask;

	//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);

	return true;
}

static void
dsm_guess_format(bool reset)
{
	static uint32_t	cs10;
	static uint32_t	cs11;
	static unsigned samples;

	/* reset the 10/11 bit sniffed channel masks */
	if (reset) {
		cs10 = 0;
		cs11 = 0;
		samples = 0;
		channel_shift = 0;
		return;
	}

	/* scan the channels in the current frame in both 10- and 11-bit mode */
	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {

		uint8_t *dp = &frame[2 + (2 * i)];
		uint16_t raw = (dp[0] << 8) | dp[1];
		unsigned channel, value;

		/* if the channel decodes, remember the assigned number */
		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
			cs10 |= (1 << channel);
		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
			cs11 |= (1 << channel);

		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
	}

	/* wait until we have seen plenty of frames - 2 should normally be enough */
	if (samples++ < 5)
		return;

	/* 
	 * Iterate the set of sensible sniffed channel sets and see whether
	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
	 */
	static uint32_t masks[] = { 
		0x3f,	/* 6 channels (DX6) */
		0x7f,	/* 7 channels (DX7) */
		0xff,	/* 8 channels (DX8) */
		0x3ff,	/* 10 channels (DX10) */
		0x3fff	/* 18 channels (DX10) */
	};
	unsigned votes10 = 0;
	unsigned votes11 = 0;

	for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {

		if (cs10 == masks[i])
			votes10++;
		if (cs11 == masks[i])
			votes11++;
	}
	if ((votes11 == 1) && (votes10 == 0)) {
		channel_shift = 11;
		debug("DSM: detected 11-bit format");
		return;
	}
	if ((votes10 == 1) && (votes11 == 0)) {
		channel_shift = 10;
		debug("DSM: detected 10-bit format");
		return;
	}

	/* call ourselves to reset our state ... we have to try again */
	debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
	dsm_guess_format(true);
}

static void
dsm_decode(hrt_abstime frame_time)
{

/*
	debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
	      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
	      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
	/*
	 * If we have lost signal for at least a second, reset the 
	 * format guessing heuristic.
	 */
	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
		dsm_guess_format(true);
	last_frame_time = frame_time;
	if (channel_shift == 0) {
		dsm_guess_format(false);
		return;
	}

	/*
	 * 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];
		unsigned channel, value;

		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
			continue;

		/* ignore channels out of range */
		if (channel >= PX4IO_INPUT_CHANNELS)
			continue;

		/* update the decoded channel count */
		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 */
		if (channel_shift == 11)
			value /= 2;
		ppm_buffer[channel] = 988 + value;
	}

	ppm_last_valid_decode = hrt_absolute_time();
}