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

S.BUS: Cleanup and code style fixes

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-07 11:41:03 +01:00
parent cde803c599
commit c78f1ace1c
2 changed files with 202 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rc/sbus.c
+201 -190
View File
@@ -77,11 +77,6 @@
#define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f #define SBUS_TARGET_MAX 2000.0f
#define SBUS_SYNC_PHASE0 0
#define SBUS_SYNC_PHASE1 1
#define SBUS_DEBUG
#ifdef SBUS_DEBUG #ifdef SBUS_DEBUG
#include <stdio.h> #include <stdio.h>
#endif #endif
@@ -113,7 +108,6 @@ static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)];
static unsigned partial_frame_count; static unsigned partial_frame_count;
static unsigned sbus_frame_drops; static unsigned sbus_frame_drops;
static unsigned sbus_sync = SBUS_SYNC_PHASE0;
unsigned unsigned
sbus_dropped_frames() sbus_dropped_frames()
@@ -123,7 +117,7 @@ sbus_dropped_frames()
static bool static bool
sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values,
bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values);
int int
sbus_init(const char *device, bool singlewire) sbus_init(const char *device, bool singlewire)
@@ -238,6 +232,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
*/ */
uint8_t buf[SBUS_FRAME_SIZE]; uint8_t buf[SBUS_FRAME_SIZE];
bool decoded = false; bool decoded = false;
do { do {
ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE); ret = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE);
@@ -249,7 +244,8 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
/* /*
* Try to decode something with what we got * 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); decoded = decoded
|| sbus_parse(now, buf, ret, values, num_values, sbus_failsafe, sbus_frame_drop, &sbus_frame_drops, max_channels);
} while (ret > 0); } while (ret > 0);
@@ -258,7 +254,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
bool bool
sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels)
{ {
last_rx_time = now; last_rx_time = now;
@@ -275,179 +271,186 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) { if (partial_frame_count == sizeof(sbus_frame) / sizeof(sbus_frame[0])) {
partial_frame_count = 0; partial_frame_count = 0;
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
#ifdef SBUS_DEBUG #ifdef SBUS_DEBUG
printf("SBUS2: RESET (BUF LIM)\n"); printf("SBUS2: RESET (BUF LIM)\n");
#endif #endif
} }
if (partial_frame_count == SBUS_FRAME_SIZE) { if (partial_frame_count == SBUS_FRAME_SIZE) {
partial_frame_count = 0; partial_frame_count = 0;
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
#ifdef SBUS_DEBUG #ifdef SBUS_DEBUG
printf("SBUS2: RESET (PACKET LIM)\n"); printf("SBUS2: RESET (PACKET LIM)\n");
#endif #endif
} }
#ifdef SBUS_DEBUG #ifdef SBUS_DEBUG
#if 0 #if 0
printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n", printf("sbus state: %s%s%s%s%s%s, count: %d, val: %02x\n",
(sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "", (sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) ? "SBUS2_DECODE_STATE_DESYNC" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "", (sbus_decode_state == SBUS2_DECODE_STATE_SBUS_START) ? "SBUS2_DECODE_STATE_SBUS_START" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "", (sbus_decode_state == SBUS2_DECODE_STATE_SBUS1_SYNC) ? "SBUS2_DECODE_STATE_SBUS1_SYNC" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "", (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_SYNC) ? "SBUS2_DECODE_STATE_SBUS2_SYNC" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "", (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "", (sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "",
partial_frame_count, partial_frame_count,
(unsigned)frame[d]); (unsigned)frame[d]);
#endif #endif
#endif #endif
switch (sbus_decode_state) { switch (sbus_decode_state) {
case SBUS2_DECODE_STATE_DESYNC: case SBUS2_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
if (frame[d] == SBUS_START_SYMBOL) { /* we are de-synced and only interested in the frame marker */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; if (frame[d] == SBUS_START_SYMBOL) {
sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
partial_frame_count = 0;
sbus_frame[partial_frame_count++] = frame[d];
}
break;
/* fall through */
case SBUS2_DECODE_STATE_SBUS_START:
case SBUS2_DECODE_STATE_SBUS1_SYNC:
/* fall through */
case SBUS2_DECODE_STATE_SBUS2_SYNC: {
sbus_frame[partial_frame_count++] = frame[d];
/* decode whatever we got and expect */
if (partial_frame_count < SBUS_FRAME_SIZE) {
break;
}
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
/*
* Offset recovery: If decoding failed, check if there is a second
* start marker in the packet.
*/
unsigned start_index = 0;
if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) {
for (unsigned i = 1; i < partial_frame_count; i++) {
if (sbus_frame[i] == SBUS_START_SYMBOL) {
start_index = i;
break;
}
}
/* we found a second start marker */
if (start_index != 0) {
/* shift everything in the buffer and reset the state machine */
for (unsigned i = 0; i < partial_frame_count - start_index; i++) {
sbus_frame[i] = sbus_frame[i + start_index];
}
partial_frame_count -= start_index;
sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
#ifdef SBUS_DEBUG
printf("DECODE RECOVERY: %d\n", start_index);
#endif
}
}
/* if there has been no successful attempt at saving a failed
* decoding run, reset the frame count for successful and
* unsuccessful decode runs.
*/
if (start_index == 0) {
partial_frame_count = 0; partial_frame_count = 0;
sbus_frame[partial_frame_count++] = frame[d];
} }
break;
/* fall through */
case SBUS2_DECODE_STATE_SBUS_START:
case SBUS2_DECODE_STATE_SBUS1_SYNC:
/* fall through */
case SBUS2_DECODE_STATE_SBUS2_SYNC:
{
sbus_frame[partial_frame_count++] = frame[d];
/* decode whatever we got and expect */
if (partial_frame_count < SBUS_FRAME_SIZE) {
break;
}
/* }
* Great, it looks like we might have a frame. Go ahead and break;
* decode it.
*/
decode_ret = sbus_decode(now, sbus_frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
/* case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: {
* Offset recovery: If decoding failed, check if there is a second sbus_frame[partial_frame_count++] = frame[d];
* start marker in the packet.
*/
unsigned start_index = 0;
if (!decode_ret && sbus_decode_state == SBUS2_DECODE_STATE_DESYNC) {
for (unsigned i = 1; i < partial_frame_count; i++) {
if (sbus_frame[i] == SBUS_START_SYMBOL) {
start_index = i;
break;
}
}
/* we found a second start marker */
if (start_index != 0) {
/* shift everything in the buffer and reset the state machine */
for (unsigned i = 0; i < partial_frame_count - start_index; i++) {
sbus_frame[i] = sbus_frame[i + start_index];
}
partial_frame_count -= start_index;
sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
#ifdef SBUS_DEBUG
printf("DECODE RECOVERY: %d\n", start_index);
#endif
}
}
/* if there has been no successful attempt at saving a failed
* decoding run, reset the frame count for successful and
* unsuccessful decode runs.
*/
if (start_index == 0) {
partial_frame_count = 0;
}
if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) {
/* this slot is unused and in fact S.BUS2 sync */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
} }
break;
case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE:
{
sbus_frame[partial_frame_count++] = frame[d];
if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) {
/* this slot is unused and in fact S.BUS2 sync */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
}
if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) {
break;
}
/* find out which payload we're dealing with in this slot */
switch(sbus_frame[0]) {
case 0x03:
{
// Observed values:
// (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0)
// (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0)
// (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f)
// (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f)
uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2];
#ifdef SBUS_DEBUG
//printf("rx_voltage %d\n", (int)rx_voltage);
#endif
}
partial_frame_count = 0;
break;
default:
/* this is not what we expect it to be, go back to sync */
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
sbus_frame_drops++;
}
if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) {
break;
} }
break;
case SBUS2_DECODE_STATE_SBUS2_GPS:
{
sbus_frame[partial_frame_count++] = frame[d];
if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) { /* find out which payload we're dealing with in this slot */
/* this slot is unused and in fact S.BUS2 sync */ switch (sbus_frame[0]) {
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; case 0x03: {
// Observed values:
// (frame[0] == 0x3 && frame[1] == 0x84 && frame[2] == 0x0)
// (frame[0] == 0x3 && frame[1] == 0xc4 && frame[2] == 0x0)
// (frame[0] == 0x3 && frame[1] == 0x80 && frame[2] == 0x2f)
// (frame[0] == 0x3 && frame[1] == 0xc0 && frame[2] == 0x2f)
#ifdef SBUS_DEBUG
//uint16_t rx_voltage = (sbus_frame[1] << 8) | sbus_frame[2];
//printf("rx_voltage %d\n", (int)rx_voltage);
#endif
} }
if (partial_frame_count < 24) { partial_frame_count = 0;
decode_ret = false; break;
break;
}
/* find out which payload we're dealing with in this slot */ default:
switch(sbus_frame[0]) { /* this is not what we expect it to be, go back to sync */
case 0x13: sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
{ sbus_frame_drops++;
uint16_t gps_something = (frame[1] << 8) | frame[2];
#ifdef SBUS_DEBUG
printf("gps_something %d\n", (int)gps_something);
#endif
}
partial_frame_count = 0;
break;
default:
/* this is not what we expect it to be, go back to sync */
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
sbus_frame_drops++;
/* throw unknown bytes away */
}
} }
break;
default: }
#ifdef SBUS_DEBUG break;
printf("UNKNOWN PROTO STATE");
#endif case SBUS2_DECODE_STATE_SBUS2_GPS: {
decode_ret = false; sbus_frame[partial_frame_count++] = frame[d];
if (partial_frame_count == 1 && sbus_frame[0] == SBUS_START_SYMBOL) {
/* this slot is unused and in fact S.BUS2 sync */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
}
if (partial_frame_count < 24) {
decode_ret = false;
break;
}
/* find out which payload we're dealing with in this slot */
switch (sbus_frame[0]) {
case 0x13: {
#ifdef SBUS_DEBUG
uint16_t gps_something = (frame[1] << 8) | frame[2];
printf("gps_something %d\n", (int)gps_something);
#endif
}
partial_frame_count = 0;
break;
default:
/* this is not what we expect it to be, go back to sync */
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
sbus_frame_drops++;
/* throw unknown bytes away */
}
}
break;
default:
#ifdef SBUS_DEBUG
printf("UNKNOWN PROTO STATE");
#endif
decode_ret = false;
} }
} }
*frame_drops = sbus_frame_drops; *frame_drops = sbus_frame_drops;
@@ -494,50 +497,57 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
bool bool
sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values,
bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{ {
/* check frame boundary markers to avoid out-of-sync cases */ /* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != SBUS_START_SYMBOL)) { if ((frame[0] != SBUS_START_SYMBOL)) {
sbus_frame_drops++; sbus_frame_drops++;
#ifdef SBUS_DEBUG #ifdef SBUS_DEBUG
printf("DECODE FAIL: "); printf("DECODE FAIL: ");
for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) { for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) {
printf("%0x ", frame[i]); printf("%0x ", frame[i]);
} }
printf("\n"); printf("\n");
#endif #endif
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
return false; return false;
} }
switch (frame[24]) { switch (frame[24]) {
case 0x00: case 0x00:
/* this is S.BUS 1 */ /* this is S.BUS 1 */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC;
break; break;
case 0x04:
/* receiver voltage */ case 0x04:
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; /* receiver voltage */
break; sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE;
case 0x14: break;
/* GPS / baro */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; case 0x14:
break; /* GPS / baro */
case 0x24: sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS;
/* Unknown SBUS2 data */ break;
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
break; case 0x24:
case 0x34: /* Unknown SBUS2 data */
/* Unknown SBUS2 data */ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; break;
break;
default: case 0x34:
#ifdef SBUS_DEBUG /* Unknown SBUS2 data */
printf("DECODE FAIL: END MARKER\n"); sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
#endif break;
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
return false; default:
#ifdef SBUS_DEBUG
printf("DECODE FAIL: END MARKER\n");
#endif
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
return false;
} }
/* we have received something we think is a frame */ /* we have received something we think is a frame */
@@ -565,7 +575,7 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
} }
/* decode switch channels if data fields are wide enough */ /* decode switch channels if data fields are wide enough */
@@ -586,16 +596,17 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
/* report that we failed to read anything valid off the receiver */ /* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true; *sbus_failsafe = true;
*sbus_frame_drop = true; *sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag /* set a special warning flag
* *
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link, * condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */ * e.g. by prematurely issueing return-to-launch!!! */
*sbus_failsafe = false; *sbus_failsafe = false;
*sbus_frame_drop = true; *sbus_frame_drop = true;
} else { } else {
*sbus_failsafe = false; *sbus_failsafe = false;
*sbus_frame_drop = false; *sbus_frame_drop = false;
src/lib/rc/sbus.h
+1 -1
View File
@@ -73,7 +73,7 @@ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bo
bool *sbus_frame_drop, bool *sbus_frame_drop,
uint16_t max_channels); uint16_t max_channels);
__EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, __EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels);
__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);