diff options
author | px4dev <px4@purgatory.org> | 2013-01-13 18:57:27 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-01-13 19:05:01 -0800 |
commit | 4e38615595abd9d27d0cb000caafb98cc3670abe (patch) | |
tree | d2b9719a348501a68821c6759d33779b6a8a325e /apps/px4io/sbus.c | |
parent | 8ebe21b27b279b5d941d4829e5ebee28b84b146c (diff) | |
download | px4-firmware-4e38615595abd9d27d0cb000caafb98cc3670abe.tar.gz px4-firmware-4e38615595abd9d27d0cb000caafb98cc3670abe.tar.bz2 px4-firmware-4e38615595abd9d27d0cb000caafb98cc3670abe.zip |
Major workover of the PX4IO firmware for I2C operation.
Diffstat (limited to 'apps/px4io/sbus.c')
-rw-r--r-- | apps/px4io/sbus.c | 42 |
1 files changed, 16 insertions, 26 deletions
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 568ef8091..d199a9361 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -66,13 +66,13 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY); + sbus_fd = open(device, O_RDONLY | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -134,7 +134,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -147,20 +147,14 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - sbus_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return sbus_decode(now, values, num_values); } /* @@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static bool +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return false; } /* if the failsafe or connection lost bit is set, we consider the frame invalid */ @@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time) (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ - system_state.rc_channels = 0; - return 1; + *values = 0; + return false; } /* we have received something we think is a frame */ @@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time) } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - system_state.rc_channel_data[channel] = (value / 2) + 998; + values[channel] = (value / 2) + 998; } /* decode switch channels if data fields are wide enough */ if (chancount > 17) { /* channel 17 (index 16) */ - system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; - - /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *num_values = chancount; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return true; } |