yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00: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-06 16:22:27 +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

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

@@ -77,11 +77,6 @@
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
#define SBUS_SYNC_PHASE0 0
#define SBUS_SYNC_PHASE1 1
#define SBUS_DEBUG
#ifdef SBUS_DEBUG
#include <stdio.h>
#endif
@@ -113,7 +108,6 @@ static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)];
static unsigned partial_frame_count;
static unsigned sbus_frame_drops;
static unsigned sbus_sync = SBUS_SYNC_PHASE0;
unsigned
sbus_dropped_frames()
@@ -123,7 +117,7 @@ sbus_dropped_frames()
static bool
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
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];
bool decoded = false;
do {
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
*/
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);
@@ -258,7 +254,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
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)
{
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])) {
partial_frame_count = 0;
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
#ifdef SBUS_DEBUG
#ifdef SBUS_DEBUG
printf("SBUS2: RESET (BUF LIM)\n");
#endif
#endif
}
if (partial_frame_count == SBUS_FRAME_SIZE) {
partial_frame_count = 0;
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
#ifdef SBUS_DEBUG
#ifdef SBUS_DEBUG
printf("SBUS2: RESET (PACKET LIM)\n");
#endif
#endif
}
#ifdef SBUS_DEBUG
#if 0
#ifdef SBUS_DEBUG
#if 0
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_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_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_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "",
partial_frame_count,
(unsigned)frame[d]);
#endif
#endif
(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_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_RX_VOLTAGE) ? "SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE" : "",
(sbus_decode_state == SBUS2_DECODE_STATE_SBUS2_GPS) ? "SBUS2_DECODE_STATE_SBUS2_GPS" : "",
partial_frame_count,
(unsigned)frame[d]);
#endif
#endif
switch (sbus_decode_state) {
case SBUS2_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
if (frame[d] == SBUS_START_SYMBOL) {
sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
case SBUS2_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
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;
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);
}
break;
/*
* 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;
}
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;
}
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) {
/* this slot is unused and in fact S.BUS2 sync */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
/* 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)
#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) {
decode_ret = false;
break;
}
partial_frame_count = 0;
break;
/* find out which payload we're dealing with in this slot */
switch(sbus_frame[0]) {
case 0x13:
{
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 */
}
default:
/* this is not what we expect it to be, go back to sync */
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
sbus_frame_drops++;
}
break;
default:
#ifdef SBUS_DEBUG
printf("UNKNOWN PROTO STATE");
#endif
decode_ret = false;
}
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) {
/* 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;
@@ -494,50 +497,57 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
bool
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 */
if ((frame[0] != SBUS_START_SYMBOL)) {
sbus_frame_drops++;
#ifdef SBUS_DEBUG
#ifdef SBUS_DEBUG
printf("DECODE FAIL: ");
for (unsigned i = 0; i < SBUS_FRAME_SIZE; i++) {
printf("%0x ", frame[i]);
}
printf("\n");
#endif
#endif
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC;
break;
case 0x04:
/* receiver voltage */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE;
break;
case 0x14:
/* GPS / baro */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS;
break;
case 0x24:
/* Unknown SBUS2 data */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
break;
case 0x34:
/* Unknown SBUS2 data */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
break;
default:
#ifdef SBUS_DEBUG
printf("DECODE FAIL: END MARKER\n");
#endif
sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
return false;
case 0x00:
/* this is S.BUS 1 */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC;
break;
case 0x04:
/* receiver voltage */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE;
break;
case 0x14:
/* GPS / baro */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS;
break;
case 0x24:
/* Unknown SBUS2 data */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
break;
case 0x34:
/* Unknown SBUS2 data */
sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
break;
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 */
@@ -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 */
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 */
@@ -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 */
*sbus_failsafe = 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
*
* 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,
*
* 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,
* e.g. by prematurely issueing return-to-launch!!! */
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;

2
src/lib/rc/sbus.h
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,
uint16_t max_channels);
__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 sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);