diff options
Diffstat (limited to 'apps/px4io/dsm.c')
-rw-r--r-- | apps/px4io/dsm.c | 184 |
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(); +} |