rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

OP-1803 S.Bus signal quality monitoring

Browse Source
This commit is contained in:
Steve Evans 2015-04-02 21:41:40 +01:00
parent a0f180d393
commit 2e5ecd2dce
4 changed files with 62 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
flight/modules/Receiver/receiver.c
View File

@ -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.
*/

4
flight/modules/Telemetry/telemetry.c
View File

@ -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,

7
flight/pios/common/pios_rcvr.c
View File

@ -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);

34
flight/pios/common/pios_sbus.c
View File

@ -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 */
}