From 2e5ecd2dce988ed31be7b980dd41b93e1ec9facd Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Thu, 2 Apr 2015 21:41:40 +0100 Subject: [PATCH] OP-1803 S.Bus signal quality monitoring --- flight/modules/Receiver/receiver.c | 61 ++++++++++++++++++---------- flight/modules/Telemetry/telemetry.c | 4 +- flight/pios/common/pios_rcvr.c | 7 ++-- flight/pios/common/pios_sbus.c | 34 ++++++++-------- 4 files changed, 62 insertions(+), 44 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 16b05c440..bdcc376a8 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -99,13 +99,17 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; - uint8_t sample_count; - uint8_t quality; + uint8_t sample_count; + uint8_t quality; }; static struct rcvr_activity_fsm activity_fsm; static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); +static void resetRcvrStatus(struct rcvr_activity_fsm *fsm); +static bool updateRcvrStatus( + struct rcvr_activity_fsm *fsm, + ManualControlSettingsChannelGroupsOptions group); #define assumptions \ ( \ @@ -140,6 +144,7 @@ int32_t ReceiverInitialize() AccessoryDesiredInitialize(); ManualControlCommandInitialize(); ReceiverActivityInitialize(); + ReceiverStatusInitialize(); ManualControlSettingsInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES StabilizationSettingsInitialize(); @@ -150,18 +155,6 @@ int32_t ReceiverInitialize() } MODULE_INITCALL(ReceiverInitialize, ReceiverStart); -static void updateRcvrStatus(uint32_t rcvr_id) -{ - extern uint32_t pios_rcvr_group_map[]; - - uint8_t quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[rcvr_id]); -#if 1 - ReceiverStatusQualitySet(&quality); -#else - quality = quality * 100 / 255; -#endif -} - /** * Module task */ @@ -187,6 +180,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) /* Initialize the RcvrActivty FSM */ portTickType lastActivityTime = xTaskGetTickCount(); resetRcvrActivity(&activity_fsm); + resetRcvrStatus(&activity_fsm); // Main task loop lastSysTime = xTaskGetTickCount(); @@ -211,9 +205,13 @@ static void receiverTask(__attribute__((unused)) void *parameters) /* Reset the aging timer because activity was detected */ lastActivityTime = lastSysTime; } + /* Read signal quality from the group used for the throttle */ + (void)updateRcvrStatus(&activity_fsm, + settings.ChannelGroups.Throttle); } if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { resetRcvrActivity(&activity_fsm); + resetRcvrStatus(&activity_fsm); lastActivityTime = lastSysTime; } @@ -257,13 +255,6 @@ static void receiverTask(__attribute__((unused)) void *parameters) } } - /* Update the receiver status, specifically signal quality, for the - * receiver used for flight mode control. - * - * SCEDEBUG Fixed to use S.Bus for now - */ - updateRcvrStatus(MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS); - // Check settings, if error raise alarm if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE @@ -529,6 +520,12 @@ static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) fsm->sample_count = 0; } +static void resetRcvrStatus(struct rcvr_activity_fsm *fsm) +{ + /* Reset the state */ + fsm->quality = 0; +} + static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { for (uint8_t channel = 1; channel <= max_channels; channel++) { @@ -599,6 +596,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { /* We're out of range, reset things */ resetRcvrActivity(fsm); + resetRcvrStatus(fsm); } extern uint32_t pios_rcvr_group_map[]; @@ -644,6 +642,27 @@ group_completed: return activity_updated; } +/* Read signal quality from the specified group */ +static bool updateRcvrStatus( + struct rcvr_activity_fsm *fsm, + ManualControlSettingsChannelGroupsOptions group) +{ + extern uint32_t pios_rcvr_group_map[]; + bool activity_updated = false; + uint8_t quality; + + quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]); + + /* Compare with previous sample */ + if (quality != fsm->quality) { + fsm->quality = quality; + ReceiverStatusQualitySet(&quality); + activity_updated = true; + } + + return activity_updated; +} + /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index ee7e53cda..0b653f652 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -221,9 +221,9 @@ int32_t TelemetryInitialize(void) timeOfLastObjectUpdate = 0; // Create object queues - localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, + localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, + radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index 66720ff5d..590e9f213 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -132,10 +132,9 @@ uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id) PIOS_Assert(0); } - if (!rcvr_dev->driver->get_quality) - { - /* If no quality is available assume max */ - return 255; + if (!rcvr_dev->driver->get_quality) { + /* If no quality is available assume max */ + return 255; } return rcvr_dev->driver->get_quality(rcvr_dev->lower_id); diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c index 5531626c8..0d6a5e02e 100644 --- a/flight/pios/common/pios_sbus.c +++ b/flight/pios/common/pios_sbus.c @@ -48,7 +48,7 @@ static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id); /* Local Variables */ const struct pios_rcvr_driver pios_sbus_rcvr_driver = { - .read = PIOS_SBus_Get, + .read = PIOS_SBus_Get, .get_quality = PIOS_SBus_Quality_Get }; @@ -129,7 +129,7 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state) state->receive_timer = 0; state->failsafe_timer = 0; state->frame_found = 0; - state->quality = 0; + state->quality = 0; PIOS_SBus_ResetChannels(state); } @@ -261,29 +261,29 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b) state->byte_count++; } else { if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) { - uint8_t quality_trend; + uint8_t quality_trend; /* full frame received */ uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3]; if (flags & SBUS_FLAG_FL) { /* frame lost, do not update */ /* Quality trend is towards 0% */ - quality_trend = 0; + quality_trend = 0; } else { /* Quality trend is towards 100% */ - quality_trend = 100; - if (flags & SBUS_FLAG_FS) { - /* failsafe flag active */ - PIOS_SBus_ResetChannels(state); - } else { - /* data looking good */ - PIOS_SBus_UnrollChannels(state); - state->failsafe_timer = 0; - } - } - /* Present quality as a weighted average of good frames */ - state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) + - quality_trend) / SBUS_FL_WEIGHTED_AVE; + quality_trend = 100; + if (flags & SBUS_FLAG_FS) { + /* failsafe flag active */ + PIOS_SBus_ResetChannels(state); + } else { + /* data looking good */ + PIOS_SBus_UnrollChannels(state); + state->failsafe_timer = 0; + } + } + /* Present quality as a weighted average of good frames */ + state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) + + quality_trend) / SBUS_FL_WEIGHTED_AVE; } else { /* discard whole frame */ }