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 | |
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')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 30 | ||||
-rw-r--r-- | apps/px4io/comms.c | 7 | ||||
-rw-r--r-- | apps/px4io/dsm.c | 154 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 5 |
4 files changed, 156 insertions, 40 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f2c87473c..11f2f6f68 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -637,10 +637,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) void PX4IO::set_rx_mode(unsigned mode) { - if (mode != _rx_mode) { - _rx_mode = mode; - _config_needed = true; - } + /* + * Always (re)set the rx mode; makes testing + * easier after PX4IO has been restarted. + */ + _rx_mode = mode; + _config_needed = true; } extern "C" __EXPORT int px4io_main(int argc, char *argv[]); @@ -744,25 +746,29 @@ px4io_main(int argc, char *argv[]) return ret; } - if (!strcmp(argv[1], "rx_dsm_10bit")) { + if (!strcmp(argv[1], "rx_dsm") || + !strcmp(argv[1], "rx_dsm_10bit") || + !strcmp(argv[1], "rx_dsm_11bit")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_10BIT); + g_dev->set_rx_mode(RX_MODE_DSM); + exit(0); } - if (!strcmp(argv[1], "rx_dsm_11bit")) { + if (!strcmp(argv[1], "rx_sbus")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_11BIT); + g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + exit(0); } - if (!strcmp(argv[1], "rx_sbus")) { + if (!strcmp(argv[1], "rx_ppm")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + g_dev->set_rx_mode(RX_MODE_PPM_ONLY); + exit(0); } if (!strcmp(argv[1], "test")) test(); - - errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f1dac433f..c44b677d1 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -112,9 +112,7 @@ serial_rx_init(unsigned serial_mode) tcgetattr(rx_fd, &t); switch (serial_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: - + case RX_MODE_DSM: /* 115200, no parity, one stop bit */ cfsetspeed(&t, 115200); t.c_cflag &= ~(CSTOPB | PARENB); @@ -163,8 +161,7 @@ comms_check(void) */ if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { switch (system_state.serial_rx_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: + case RX_MODE_DSM: dsm_input(rx_fd); break; 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(); } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 660eed12b..8d051d385 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -64,9 +64,8 @@ struct px4io_config { uint8_t serial_rx_mode; #define RX_MODE_PPM_ONLY 0 -#define RX_MODE_DSM_10BIT 1 -#define RX_MODE_DSM_11BIT 2 -#define RX_MODE_FUTABA_SBUS 3 +#define RX_MODE_DSM 1 +#define RX_MODE_FUTABA_SBUS 2 }; /* report from IO to FMU */ |