forked from Archive/PX4-Autopilot
Code style for DSM decoder
This commit is contained in:
Lorenz Meier
parent
7ed3fbb1bc
commit
6e3749f904
310
src/lib/rc/dsm.c
310
src/lib/rc/dsm.c
|
|
@ -51,17 +51,17 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
|
||||
#define dsm_udelay(arg) usleep(arg)
|
||||
#define dsm_udelay(arg) usleep(arg)
|
||||
#else
|
||||
#include <nuttx/arch.h>
|
||||
#define dsm_udelay(arg) up_udelay(arg)
|
||||
#include <nuttx/arch.h>
|
||||
#define dsm_udelay(arg) up_udelay(arg)
|
||||
#endif
|
||||
|
||||
//#define DSM_DEBUG
|
||||
|
||||
static enum DSM_DECODE_STATE {
|
||||
DSM_DECODE_STATE_DESYNC = 0,
|
||||
DSM_DECODE_STATE_SYNC
|
||||
DSM_DECODE_STATE_DESYNC = 0,
|
||||
DSM_DECODE_STATE_SYNC
|
||||
} dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
|
||||
static int dsm_fd = -1; /**< File handle to the DSM UART */
|
||||
|
|
@ -128,9 +128,9 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
|
|||
static bool
|
||||
dsm_guess_format(bool reset)
|
||||
{
|
||||
static uint32_t cs10 = 0;
|
||||
static uint32_t cs11 = 0;
|
||||
static unsigned samples = 0;
|
||||
static uint32_t cs10 = 0;
|
||||
static uint32_t cs11 = 0;
|
||||
static unsigned samples = 0;
|
||||
|
||||
/* reset the 10/11 bit sniffed channel masks */
|
||||
if (reset) {
|
||||
|
|
@ -138,7 +138,7 @@ dsm_guess_format(bool reset)
|
|||
cs11 = 0;
|
||||
samples = 0;
|
||||
dsm_channel_shift = 0;
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
|
||||
|
|
@ -160,16 +160,16 @@ dsm_guess_format(bool reset)
|
|||
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
|
||||
}
|
||||
|
||||
samples++;
|
||||
samples++;
|
||||
|
||||
#ifdef DSM_DEBUG
|
||||
printf("dsm guess format: samples: %d %s\n", samples,
|
||||
(reset) ? "RESET" : "");
|
||||
printf("dsm guess format: samples: %d %s\n", samples,
|
||||
(reset) ? "RESET" : "");
|
||||
#endif
|
||||
|
||||
/* wait until we have seen plenty of frames - 5 should normally be enough */
|
||||
if (samples < 5) {
|
||||
return false;
|
||||
if (samples < 5) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -209,25 +209,25 @@ dsm_guess_format(bool reset)
|
|||
if ((votes11 == 1) && (votes10 == 0)) {
|
||||
dsm_channel_shift = 11;
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: 11-bit format\n");
|
||||
printf("DSM: 11-bit format\n");
|
||||
#endif
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if ((votes10 == 1) && (votes11 == 0)) {
|
||||
dsm_channel_shift = 10;
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: 10-bit format\n");
|
||||
printf("DSM: 10-bit format\n");
|
||||
#endif
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* call ourselves to reset our state ... we have to try again */
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11);
|
||||
printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11);
|
||||
#endif
|
||||
dsm_guess_format(true);
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -278,10 +278,10 @@ dsm_init(const char *device)
|
|||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
}
|
||||
|
||||
dsm_channel_shift = 0;
|
||||
dsm_frame_drops = 0;
|
||||
dsm_chan_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
dsm_channel_shift = 0;
|
||||
dsm_frame_drops = 0;
|
||||
dsm_chan_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
|
||||
int ret = dsm_config(dsm_fd);
|
||||
|
||||
|
|
@ -325,17 +325,17 @@ dsm_bind(uint16_t cmd, int pulses)
|
|||
case DSM_CMD_BIND_SET_RX_OUT:
|
||||
|
||||
/*Set UART RX pin to active output mode*/
|
||||
SPEKTRUM_RX_AS_GPIO();
|
||||
SPEKTRUM_RX_AS_GPIO();
|
||||
break;
|
||||
|
||||
case DSM_CMD_BIND_SEND_PULSES:
|
||||
|
||||
/*Pulse RX pin a number of times*/
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
dsm_udelay(120);
|
||||
SPEKTRUM_RX_HIGH(false);
|
||||
dsm_udelay(120);
|
||||
SPEKTRUM_RX_HIGH(true);
|
||||
dsm_udelay(120);
|
||||
SPEKTRUM_RX_HIGH(false);
|
||||
dsm_udelay(120);
|
||||
SPEKTRUM_RX_HIGH(true);
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -343,7 +343,7 @@ dsm_bind(uint16_t cmd, int pulses)
|
|||
case DSM_CMD_BIND_REINIT_UART:
|
||||
|
||||
/*Restore USART RX pin to RS232 receive mode*/
|
||||
SPEKTRUM_RX_AS_UART();
|
||||
SPEKTRUM_RX_AS_UART();
|
||||
break;
|
||||
|
||||
}
|
||||
|
|
@ -370,15 +370,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
|
|||
* If we have lost signal for at least a second, reset the
|
||||
* format guessing heuristic.
|
||||
*/
|
||||
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
|
||||
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
|
||||
dsm_guess_format(true);
|
||||
}
|
||||
|
||||
/* if we don't know the dsm_frame format, update the guessing state machine */
|
||||
if (dsm_channel_shift == 0) {
|
||||
if (!dsm_guess_format(false)) {
|
||||
return false;
|
||||
}
|
||||
if (!dsm_guess_format(false)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -402,11 +402,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
|
|||
continue;
|
||||
}
|
||||
|
||||
/* reset bit guessing state machine if the channel index is out of bounds */
|
||||
if (channel > DSM_MAX_CHANNEL_COUNT) {
|
||||
dsm_guess_format(true);
|
||||
return false;
|
||||
}
|
||||
/* reset bit guessing state machine if the channel index is out of bounds */
|
||||
if (channel > DSM_MAX_CHANNEL_COUNT) {
|
||||
dsm_guess_format(true);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ignore channels out of range */
|
||||
if (channel >= max_values) {
|
||||
|
|
@ -475,31 +475,31 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
|
|||
*num_values = 12;
|
||||
}
|
||||
|
||||
/* Set the 11-bit data indicator */
|
||||
*dsm_11_bit = (dsm_channel_shift == 11);
|
||||
/* Set the 11-bit data indicator */
|
||||
*dsm_11_bit = (dsm_channel_shift == 11);
|
||||
|
||||
/* we have received something we think is a dsm_frame */
|
||||
dsm_last_frame_time = frame_time;
|
||||
/* we have received something we think is a dsm_frame */
|
||||
dsm_last_frame_time = frame_time;
|
||||
|
||||
/*
|
||||
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
|
||||
*/
|
||||
|
||||
#ifdef DSM_DEBUG
|
||||
printf("PARSED PACKET\n");
|
||||
printf("PARSED PACKET\n");
|
||||
#endif
|
||||
|
||||
/* check all values */
|
||||
for (unsigned i = 0; i < *num_values; i++) {
|
||||
/* if the value is unrealistic, fail the parsing entirely */
|
||||
if (values[i] < 600 || values[i] > 2400) {
|
||||
/* check all values */
|
||||
for (unsigned i = 0; i < *num_values; i++) {
|
||||
/* if the value is unrealistic, fail the parsing entirely */
|
||||
if (values[i] < 600 || values[i] > 2400) {
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: VALUE RANGE FAIL\n");
|
||||
printf("DSM: VALUE RANGE FAIL\n");
|
||||
#endif
|
||||
dsm_chan_count = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
dsm_chan_count = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -527,148 +527,148 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
|
|||
*/
|
||||
bool
|
||||
dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes,
|
||||
unsigned max_values)
|
||||
unsigned max_values)
|
||||
{
|
||||
int ret = 1;
|
||||
hrt_abstime now;
|
||||
int ret = 1;
|
||||
hrt_abstime now;
|
||||
|
||||
/*
|
||||
* The S.BUS protocol doesn't provide reliable framing,
|
||||
* so we detect frame boundaries by the inter-frame delay.
|
||||
*
|
||||
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
|
||||
* frame transmission time is ~2ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 3ms 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();
|
||||
/*
|
||||
* The S.BUS protocol doesn't provide reliable framing,
|
||||
* so we detect frame boundaries by the inter-frame delay.
|
||||
*
|
||||
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
|
||||
* frame transmission time is ~2ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 3ms 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();
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* a complete frame.
|
||||
*/
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* a complete frame.
|
||||
*/
|
||||
|
||||
ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
|
||||
ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1) {
|
||||
return false;
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1) {
|
||||
return false;
|
||||
|
||||
} else {
|
||||
*n_bytes = ret;
|
||||
*bytes = &dsm_buf[0];
|
||||
}
|
||||
} else {
|
||||
*n_bytes = ret;
|
||||
*bytes = &dsm_buf[0];
|
||||
}
|
||||
|
||||
/*
|
||||
* Try to decode something with what we got
|
||||
*/
|
||||
return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values);
|
||||
/*
|
||||
* Try to decode something with what we got
|
||||
*/
|
||||
return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values);
|
||||
}
|
||||
|
||||
bool
|
||||
dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
|
||||
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
|
||||
{
|
||||
|
||||
/* this is set by the decoding state machine and will default to false
|
||||
* once everything that was decodable has been decoded.
|
||||
*/
|
||||
bool decode_ret = false;
|
||||
/* this is set by the decoding state machine and will default to false
|
||||
* once everything that was decodable has been decoded.
|
||||
*/
|
||||
bool decode_ret = false;
|
||||
|
||||
/* keep decoding until we have consumed the buffer */
|
||||
for (unsigned d = 0; d < len; d++) {
|
||||
/* keep decoding until we have consumed the buffer */
|
||||
for (unsigned d = 0; d < len; d++) {
|
||||
|
||||
/* overflow check */
|
||||
if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
/* overflow check */
|
||||
if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: RESET (BUF LIM)\n");
|
||||
printf("DSM: RESET (BUF LIM)\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
#ifdef DSM_DEBUG
|
||||
printf("DSM: RESET (PACKET LIM)\n");
|
||||
printf("DSM: RESET (PACKET LIM)\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DSM_DEBUG
|
||||
#if 1
|
||||
printf("dsm state: %s%s, count: %d, val: %02x\n",
|
||||
(dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
|
||||
(dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
|
||||
dsm_partial_frame_count,
|
||||
(unsigned)frame[d]);
|
||||
printf("dsm state: %s%s, count: %d, val: %02x\n",
|
||||
(dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
|
||||
(dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
|
||||
dsm_partial_frame_count,
|
||||
(unsigned)frame[d]);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
switch (dsm_decode_state) {
|
||||
case DSM_DECODE_STATE_DESYNC:
|
||||
switch (dsm_decode_state) {
|
||||
case DSM_DECODE_STATE_DESYNC:
|
||||
|
||||
/* we are de-synced and only interested in the frame marker */
|
||||
if ((now - dsm_last_rx_time) > 5000) {
|
||||
dsm_decode_state = DSM_DECODE_STATE_SYNC;
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_chan_count = 0;
|
||||
dsm_frame[dsm_partial_frame_count++] = frame[d];
|
||||
}
|
||||
/* we are de-synced and only interested in the frame marker */
|
||||
if ((now - dsm_last_rx_time) > 5000) {
|
||||
dsm_decode_state = DSM_DECODE_STATE_SYNC;
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_chan_count = 0;
|
||||
dsm_frame[dsm_partial_frame_count++] = frame[d];
|
||||
}
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
case DSM_DECODE_STATE_SYNC: {
|
||||
dsm_frame[dsm_partial_frame_count++] = frame[d];
|
||||
case DSM_DECODE_STATE_SYNC: {
|
||||
dsm_frame[dsm_partial_frame_count++] = frame[d];
|
||||
|
||||
/* decode whatever we got and expect */
|
||||
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
|
||||
break;
|
||||
}
|
||||
/* decode whatever we got and expect */
|
||||
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels);
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels);
|
||||
|
||||
/* we consumed the partial frame, reset */
|
||||
dsm_partial_frame_count = 0;
|
||||
/* we consumed the partial frame, reset */
|
||||
dsm_partial_frame_count = 0;
|
||||
|
||||
/* if decoding failed, set proto to desync */
|
||||
if (decode_ret == false) {
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
dsm_frame_drops++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
/* if decoding failed, set proto to desync */
|
||||
if (decode_ret == false) {
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
dsm_frame_drops++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
default:
|
||||
#ifdef DSM_DEBUG
|
||||
printf("UNKNOWN PROTO STATE");
|
||||
printf("UNKNOWN PROTO STATE");
|
||||
#endif
|
||||
decode_ret = false;
|
||||
}
|
||||
decode_ret = false;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (frame_drops) {
|
||||
*frame_drops = dsm_frame_drops;
|
||||
}
|
||||
if (frame_drops) {
|
||||
*frame_drops = dsm_frame_drops;
|
||||
}
|
||||
|
||||
if (decode_ret) {
|
||||
*num_values = dsm_chan_count;
|
||||
}
|
||||
if (decode_ret) {
|
||||
*num_values = dsm_chan_count;
|
||||
}
|
||||
|
||||
dsm_last_rx_time = now;
|
||||
dsm_last_rx_time = now;
|
||||
|
||||
/* return false as default */
|
||||
return decode_ret;
|
||||
/* return false as default */
|
||||
return decode_ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -55,10 +55,11 @@ __BEGIN_DECLS
|
|||
|
||||
__EXPORT int dsm_init(const char *device);
|
||||
__EXPORT int dsm_config(int dsm_fd);
|
||||
__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values);
|
||||
__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
|
||||
uint8_t **bytes, unsigned max_values);
|
||||
|
||||
__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
|
||||
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
|
||||
|
||||
#ifdef GPIO_SPEKTRUM_PWR_EN
|
||||
__EXPORT void dsm_bind(uint16_t cmd, int pulses);
|
||||
|
|
|
|||
Loading…
Reference in New Issue