diff options
Diffstat (limited to 'apps/px4io/sbus.c')
| -rw-r--r-- | apps/px4io/sbus.c | 16 |
1 files changed, 13 insertions, 3 deletions
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 25fe0cf38..a8f628a84 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -58,6 +58,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -94,7 +95,7 @@ sbus_init(const char *device) return sbus_fd; } -void +bool sbus_input(void) { ssize_t ret; @@ -131,7 +132,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -143,7 +144,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -151,6 +152,12 @@ sbus_input(void) */ 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; } /* @@ -203,6 +210,9 @@ sbus_decode(hrt_abstime frame_time) return; } + /* we have received something we think is a frame */ + last_frame_time = frame_time; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; |