diff options
author | px4dev <px4@purgatory.org> | 2012-11-29 00:35:21 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-11-29 00:35:21 -0800 |
commit | d0efd1a419497ec2fadb7b516cd2f9cc5a09ca5d (patch) | |
tree | 306580e8d7665608915cfb3d7ffb9e75dd0119c4 /apps/px4io/dsm.c | |
parent | 3321ca08886fe1030c82094254586c31fcb07b32 (diff) | |
download | px4-firmware-d0efd1a419497ec2fadb7b516cd2f9cc5a09ca5d.tar.gz px4-firmware-d0efd1a419497ec2fadb7b516cd2f9cc5a09ca5d.tar.bz2 px4-firmware-d0efd1a419497ec2fadb7b516cd2f9cc5a09ca5d.zip |
Fix the DSM (spektrum) protocol decoder, and add some format auto-detection to it.
Diffstat (limited to 'apps/px4io/dsm.c')
-rw-r--r-- | apps/px4io/dsm.c | 154 |
1 files changed, 134 insertions, 20 deletions
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 79b6301c7..4384ab523 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -49,12 +49,15 @@ #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]; @@ -63,21 +66,21 @@ static unsigned partial_frame_count; static bool insync; static unsigned channel_shift; -static void dsm_decode(void); +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(); - if (mode == RX_MODE_DSM_10BIT) { - channel_shift = 10; - } else { - channel_shift = 11; - } + /* reset the format detector */ + dsm_guess_format(true); - last_frame_time = hrt_absolute_time(); + debug("DSM: enabled and waiting\n"); } void @@ -97,10 +100,17 @@ dsm_input(int fd) * 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_frame_time) > 5000) + 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 @@ -111,6 +121,7 @@ dsm_input(int fd) /* 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 @@ -123,20 +134,125 @@ dsm_input(int fd) */ 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(); + 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_decode(void) +dsm_guess_format(bool reset) { - uint16_t data_mask = (1 << channel_shift) - 1; + 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 @@ -159,26 +275,24 @@ dsm_decode(void) uint8_t *dp = &frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - /* ignore pad channels */ - if (raw == 0xffff) + if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) continue; - unsigned channel = (raw >> channel_shift) & 0xf; - /* 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 */ - unsigned data = raw & data_mask; if (channel_shift == 11) - data /= 2; - ppm_buffer[channel] = 988 + data; - + value /= 2; + ppm_buffer[channel] = 988 + value; } + ppm_last_valid_decode = hrt_absolute_time(); } |