fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 04:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

SBUS2 parser: Last set of fixes, validated in test and against Futaba TX

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-07 11:41:03 +01:00
parent 3be7a3323e
commit 6462deebad
2 changed files with 19 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rc/sbus.c
+19 -27
View File
@@ -49,8 +49,6 @@
#define SBUS_START_SYMBOL 0x0f #define SBUS_START_SYMBOL 0x0f
#define SBUS_FRAME_SIZE 25
#define SBUS_MAX_BUF_SIZE (SBUS_FRAME_SIZE + 10)
#define SBUS_INPUT_CHANNELS 16 #define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23 #define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3 #define SBUS_FAILSAFE_BIT 3
@@ -199,7 +197,7 @@ bool
sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_channels) uint16_t max_channels)
{ {
ssize_t ret; int ret = 1;
hrt_abstime now; hrt_abstime now;
/* /*
@@ -219,37 +217,30 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > SBUS_INTER_FRAME_TIMEOUT) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
partial_frame_count = 0;
}
}
/* /*
* Fetch bytes, but no more than we would need to complete * Fetch bytes, but no more than we would need to complete
* a complete frame. * a complete frame.
*/ */
uint8_t buf[SBUS_FRAME_SIZE]; uint8_t buf[SBUS_FRAME_SIZE * 2];
bool decoded = false; bool sbus_decoded = false;
do { ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE);
ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE);
/* if the read failed for any reason, just give up here */ /* if the read failed for any reason, just give up here */
if (ret < 1) { if (ret < 1) {
return false; return false;
} }
/* /*
* Try to decode something with what we got * Try to decode something with what we got
*/ */
decoded = decoded if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe,
|| sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels); sbus_frame_drop, &sbus_frame_drops, max_channels)) {
} while (ret > 0); sbus_decoded = true;
}
return decoded; return sbus_decoded;
} }
bool bool
@@ -418,7 +409,6 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
} }
if (partial_frame_count < 24) { if (partial_frame_count < 24) {
decode_ret = false;
break; break;
} }
@@ -453,7 +443,9 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
} }
*frame_drops = sbus_frame_drops; if (frame_drops) {
*frame_drops = sbus_frame_drops;
}
/* return false as default */ /* return false as default */
return decode_ret; return decode_ret;
src/lib/rc/sbus.h
-1
View File
@@ -46,7 +46,6 @@
__BEGIN_DECLS __BEGIN_DECLS
#define SBUS_INTER_FRAME_TIMEOUT 3000 /**< 3000 us frame timeout */
#define SBUS_FRAME_SIZE 25 #define SBUS_FRAME_SIZE 25
#define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2) #define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2)