diff options
Diffstat (limited to 'src/modules/px4iofirmware/sbus.c')
-rw-r--r-- | src/modules/px4iofirmware/sbus.c | 17 |
1 files changed, 10 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 68af85dc9..2153fadc8 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, max_channels); + return sbus_decode(now, values, num_values, rssi, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -266,8 +266,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ - values[2] = 900; + /* report that we failed to read anything valid off the receiver */ + *rssi = 0; + return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented @@ -276,8 +277,10 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - // values[2] = 888; // marker for debug purposes + *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } + *rssi = 1000; + return true; } |