diff options
author | Jean Cyr <jcyr@dillobits.com> | 2013-08-15 19:51:36 -0400 |
---|---|---|
committer | Jean Cyr <jcyr@dillobits.com> | 2013-08-15 19:51:36 -0400 |
commit | ab80b0e273d5b2d795c8b5f470f773052cbaeedb (patch) | |
tree | f1c0e89cc50244f5403e3f0dd79a735bb261b0be /src/modules | |
parent | 5142a9ffda89af41c8d5d5a8d3c1839a93c43555 (diff) | |
download | px4-firmware-ab80b0e273d5b2d795c8b5f470f773052cbaeedb.tar.gz px4-firmware-ab80b0e273d5b2d795c8b5f470f773052cbaeedb.tar.bz2 px4-firmware-ab80b0e273d5b2d795c8b5f470f773052cbaeedb.zip |
Doxygenate and style dsm.c
One file a day... this'll take a while!
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 412 |
1 files changed, 243 insertions, 169 deletions
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index b2c0db425..e014b0a51 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -48,158 +48,55 @@ #include <drivers/drv_hrt.h> -#define DEBUG - #include "px4io.h" +//! The DSM dsm frame size in bytes #define DSM_FRAME_SIZE 16 +//! Maximum supported DSM channels #define DSM_FRAME_CHANNELS 7 -static int dsm_fd = -1; - -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 unsigned channel_shift; - +//! File handle to the DSM UART +int dsm_fd = -1; +//! Timestamp when we last received +hrt_abstime dsm_last_rx_time; +//! Timestamp for start of last dsm_frame +hrt_abstime dsm_last_frame_time; +//! DSM dsm_frame receive buffer +uint8_t dsm_frame[DSM_FRAME_SIZE]; +//! Count of bytes received for current dsm_frame +unsigned dsm_partial_frame_count; +//! Channel resolution, 0=unknown, 1=10 bit, 2=11 bit +unsigned dsm_channel_shift; +//! Count of incomplete DSM frames unsigned dsm_frame_drops; -static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); -static void dsm_guess_format(bool reset); -static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); - -int -dsm_init(const char *device) -{ - if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY | O_NONBLOCK); - - if (dsm_fd >= 0) { - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(dsm_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(dsm_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - /* reset the format detector */ - dsm_guess_format(true); - - debug("DSM: ready"); - - } else { - debug("DSM: open failed"); - } - - return dsm_fd; -} - -void -dsm_bind(uint16_t cmd, int pulses) -{ - const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10; - - if (dsm_fd < 0) - return; - - switch (cmd) { - case dsm_bind_power_down: - // power down DSM satellite - POWER_RELAY1(0); - break; - case dsm_bind_power_up: - POWER_RELAY1(1); - dsm_guess_format(true); - break; - case dsm_bind_set_rx_out: - stm32_configgpio(usart1RxAsOutp); - break; - case dsm_bind_send_pulses: - for (int i = 0; i < pulses; i++) { - stm32_gpiowrite(usart1RxAsOutp, false); - up_udelay(25); - stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); - } - break; - case dsm_bind_reinit_uart: - // Restore USART rx pin - stm32_configgpio(GPIO_USART1_RX); - break; - } -} - -bool -dsm_input(uint16_t *values, uint16_t *num_values) -{ - 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) { - dsm_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return dsm_decode(now, values, num_values); -} - -static bool -dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) +/** + * Attempt to decode a single channel raw channel datum + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm_frame boundaries by the inter-dsm_frame delay. + * + * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps + * dsm_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 dsm_frame. + * + * In the case where byte(s) are dropped from a dsm_frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * + * Upon receiving a full dsm_frame we attempt to decode it + * + * @param[in] raw 16 bit raw channel value from dsm_frame + * @param[in] shift position of channel number in raw data + * @param[out] channel pointer to returned channel number + * @param[out] value pointer to returned channel value + * @return true=raw value successfully decoded + */ +bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); +bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { if (raw == 0xffff) @@ -215,8 +112,13 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va return true; } -static void -dsm_guess_format(bool reset) +/** + * Attempt to guess if receiving 10 or 11 bit channel values + * + * @param[in] reset true=reset the 10/11 bit state to unknow + */ +void dsm_guess_format(bool reset); +void dsm_guess_format(bool reset) { static uint32_t cs10; static uint32_t cs11; @@ -227,14 +129,14 @@ dsm_guess_format(bool reset) cs10 = 0; cs11 = 0; samples = 0; - channel_shift = 0; + dsm_channel_shift = 0; return; } - /* scan the channels in the current frame in both 10- and 11-bit mode */ + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; @@ -245,10 +147,10 @@ dsm_guess_format(bool reset) 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 */ + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - /* wait until we have seen plenty of frames - 2 should normally be enough */ + /* wait until we have seen plenty of frames - 5 should normally be enough */ if (samples++ < 5) return; @@ -284,13 +186,13 @@ dsm_guess_format(bool reset) } if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; + dsm_channel_shift = 11; debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; + dsm_channel_shift = 10; debug("DSM: 10-bit format"); return; } @@ -300,27 +202,129 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } -static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int dsm_init(const char *device) { + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + if (dsm_fd >= 0) { + + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + dsm_partial_frame_count = 0; + dsm_last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + + } else { + + debug("DSM: open failed"); + + } + + return dsm_fd; +} + +/** + * Handle DSM satellite receiver bind mode handler + * + * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart + * @param[in] pulses Number of pulses for dsm_bind_send_pulses command + */ +void dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = + GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + + case dsm_bind_power_down: + + //! power down DSM satellite + POWER_RELAY1(0); + break; + + case dsm_bind_power_up: + + //! power up DSM satellite + POWER_RELAY1(1); + dsm_guess_format(true); + break; + + case dsm_bind_set_rx_out: + + //! Set UART RX pin to active output mode + stm32_configgpio(usart1RxAsOutp); + break; + + case dsm_bind_send_pulses: + + //! Pulse RX pin a number of times + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(25); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(25); + } + break; + + case dsm_bind_reinit_uart: + + //! Restore USART RX pin to RS232 receive mode + stm32_configgpio(GPIO_USART1_RX); + break; + + } +} + +/** + * Decode the entire dsm_frame (all contained channels) + * + * @param[in] frame_time timestamp when this dsm_frame was received. Used to detect RX loss in order to reset 10/11 bit guess. + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=dsm_frame successfully decoded + */ +bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +{ /* - 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]); + debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], + dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_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)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) dsm_guess_format(true); - /* we have received something we think is a frame */ - last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; - /* if we don't know the frame format, update the guessing state machine */ - if (channel_shift == 0) { + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (dsm_channel_shift == 0) { dsm_guess_format(false); return false; } @@ -332,17 +336,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * 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 + * second dsm_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)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) continue; /* ignore channels out of range */ @@ -354,7 +358,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) *num_values = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (channel_shift == 11) + if (dsm_channel_shift == 11) value /= 2; value += 998; @@ -385,7 +389,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (channel_shift == 11) + if (dsm_channel_shift == 11) *num_values |= 0x8000; /* @@ -393,3 +397,73 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) */ return true; } + +/** + * Called periodically to check for input data from the DSM UART + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm_frame boundaries by the inter-dsm_frame delay. + * + * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps + * dsm_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 dsm_frame. + * + * In the case where byte(s) are dropped from a dsm_frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * + * Upon receiving a full dsm_frame we attempt to decode it + * + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=decode raw channel values updated + */ +bool dsm_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + */ + now = hrt_absolute_time(); + + if ((now - dsm_last_rx_time) > 5000) { + if (dsm_partial_frame_count > 0) { + dsm_frame_drops++; + dsm_partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current dsm_frame. + */ + ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + dsm_last_rx_time = now; + + /* + * Add bytes to the current dsm_frame + */ + dsm_partial_frame_count += ret; + + /* + * If we don't have a full dsm_frame, return + */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a dsm_frame. Go ahead and + * decode it. + */ + dsm_partial_frame_count = 0; + return dsm_decode(now, values, num_values); +} |