fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06 17:36:12 +01:00
parent 3be7a3323e
commit 6462deebad
2 changed files with 19 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/lib/rc/sbus.c
View File

@ -49,8 +49,6 @@
#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_FLAGS_BYTE 23
#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,
uint16_t max_channels)
{
ssize_t ret;
int ret = 1;
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();
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
* a complete frame.
*/
uint8_t buf[SBUS_FRAME_SIZE];
bool decoded = false;
uint8_t buf[SBUS_FRAME_SIZE * 2];
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 (ret < 1) {
return false;
}
/* if the read failed for any reason, just give up here */
if (ret < 1) {
return false;
}
/*
* Try to decode something with what we got
*/
decoded = decoded
|| sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels);
/*
* Try to decode something with what we got
*/
if (sbus_parse(now, &buf[0], ret, values, num_values, sbus_failsafe,
sbus_frame_drop, &sbus_frame_drops, max_channels)) {
} while (ret > 0);
sbus_decoded = true;
}
return decoded;
return sbus_decoded;
}
bool
@ -418,7 +409,6 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
}
if (partial_frame_count < 24) {
decode_ret = false;
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 decode_ret;

1
src/lib/rc/sbus.h
View File

@ -46,7 +46,6 @@
__BEGIN_DECLS
#define SBUS_INTER_FRAME_TIMEOUT 3000 /**< 3000 us frame timeout */
#define SBUS_FRAME_SIZE 25
#define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2)