rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-196 Adding signal quality calculation to Ex.Bus receiver module.

Browse Source
This commit is contained in:
Fredrik Arvidsson 2016-01-25 00:23:31 +01:00
parent e17d809120
commit ffa671da78
1 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/pios/common/pios_exbus.c
View File

@ -60,6 +60,11 @@
#define EXBUS_FRAME_TIMEOUT 4 #define EXBUS_FRAME_TIMEOUT 4
#define EXBUS_FAILSAFE_TIMEOUT 64 #define EXBUS_FAILSAFE_TIMEOUT 64
/* With an Ex.Bus frame rate of 10ms (100Hz) averaging over 20 samples
* gives about a 200ms response.
*/
#define EXBUS_FL_WEIGHTED_AVERAGE 20
/* Forward Declarations */ /* Forward Declarations */
static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel); static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel);
static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context,
@ -69,10 +74,12 @@ static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context,
bool *need_yield); bool *need_yield);
static void PIOS_EXBUS_Supervisor(uint32_t exbus_id); static void PIOS_EXBUS_Supervisor(uint32_t exbus_id);
static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data); static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data);
static uint8_t PIOS_EXBUS_Quality_Get(uint32_t rcvr_id);
/* Local Variables */ /* Local Variables */
const struct pios_rcvr_driver pios_exbus_rcvr_driver = { const struct pios_rcvr_driver pios_exbus_rcvr_driver = {
.read = PIOS_EXBUS_Get, .read = PIOS_EXBUS_Get,
.get_quality = PIOS_EXBUS_Quality_Get,
}; };
enum pios_exbus_dev_magic { enum pios_exbus_dev_magic {
@ -100,6 +107,7 @@ struct pios_exbus_state {
uint16_t crc; uint16_t crc;
bool high_baud_rate; bool high_baud_rate;
bool frame_found; bool frame_found;
float quality;
}; };
struct pios_exbus_dev { struct pios_exbus_dev {
@ -149,6 +157,7 @@ static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev)
state->failsafe_count = 0; state->failsafe_count = 0;
state->high_baud_rate = false; state->high_baud_rate = false;
state->frame_found = false; state->frame_found = false;
state->quality = 0.0f;
PIOS_EXBUS_ResetChannels(exbus_dev); PIOS_EXBUS_ResetChannels(exbus_dev);
} }
@ -261,11 +270,16 @@ static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byt
state->frame_length = byte; state->frame_length = byte;
} }
if (state->byte_count == state->frame_length) { if (state->byte_count == state->frame_length) {
uint8_t quality_trend = 0;
if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) { if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) {
/* data looking good */ /* data looking good */
state->failsafe_timer = 0; state->failsafe_timer = 0;
state->failsafe_count = 0; state->failsafe_count = 0;
quality_trend = 100;
} }
// Calculate quality trend using weighted average of good frames
state->quality = ((state->quality * (EXBUS_FL_WEIGHTED_AVERAGE - 1)) +
quality_trend) / EXBUS_FL_WEIGHTED_AVERAGE;
/* get ready for the next frame */ /* get ready for the next frame */
state->frame_found = false; state->frame_found = false;
} }
@ -407,10 +421,24 @@ static void PIOS_EXBUS_Supervisor(uint32_t exbus_id)
if (++state->failsafe_timer > EXBUS_FAILSAFE_TIMEOUT) { if (++state->failsafe_timer > EXBUS_FAILSAFE_TIMEOUT) {
PIOS_EXBUS_ResetChannels(exbus_dev); PIOS_EXBUS_ResetChannels(exbus_dev);
state->failsafe_timer = 0; state->failsafe_timer = 0;
state->quality = 0.0f;
PIOS_EXBUS_Change_BaudRate(exbus_dev); PIOS_EXBUS_Change_BaudRate(exbus_dev);
} }
} }
static uint8_t PIOS_EXBUS_Quality_Get(uint32_t exbus_id)
{
struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)exbus_id;
bool valid = PIOS_EXBUS_Validate(exbus_dev);
PIOS_Assert(valid);
struct pios_exbus_state *state = &(exbus_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data) static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data)
{ {
data ^= (uint8_t)crc & (uint8_t)0xFF; data ^= (uint8_t)crc & (uint8_t)0xFF;