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

First stab at implementing better RSSI based connection status estimation, still needs some work and testing

Browse Source
This commit is contained in:
Lorenz Meier
2013-12-13 18:01:55 +01:00
parent a673fa3926
commit 9883a346a0
6 changed files with 33 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/sbus.c
+10 -7
View File
@@ -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;
}