1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00

Merge branch 'steve/OP-1803_s.bus_dropped_frames' into next

This commit is contained in:
abeck70 2015-05-14 17:24:58 +10:00
commit 5b11dbbf67
12 changed files with 254 additions and 41 deletions

View File

@ -37,6 +37,7 @@
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <receiveractivity.h>
#include <receiverstatus.h>
#include <flightstatus.h>
#include <flighttelemetrystats.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
@ -102,12 +103,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 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 \
( \
@ -143,6 +149,7 @@ int32_t ReceiverInitialize()
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
ReceiverActivityInitialize();
ReceiverStatusInitialize();
ManualControlSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
StabilizationSettingsInitialize();
@ -208,6 +215,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();
@ -232,9 +240,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;
}
@ -278,6 +290,10 @@ static void receiverTask(__attribute__((unused)) void *parameters)
}
}
/* Read signal quality from the group used for the throttle */
(void)updateRcvrStatus(&activity_fsm,
settings.ChannelGroups.Throttle);
// Sanity Check Throttle and Yaw
if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
@ -568,6 +584,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++) {
@ -641,6 +663,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[];
@ -686,6 +709,32 @@ 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;
int8_t quality;
quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
/* If no driver is detected or any other error then return */
if (quality < 0) {
return activity_updated;
}
/* Compare with previous sample */
if (quality != fsm->quality) {
fsm->quality = quality;
ReceiverStatusQualitySet(&fsm->quality);
activity_updated = true;
}
return activity_updated;
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/

View File

@ -34,6 +34,7 @@
#include <uavobjectmanager.h>
#include <oplinkreceiver.h>
#include <oplinkstatus.h>
#include <pios_oplinkrcvr_priv.h>
static OPLinkReceiverData oplinkreceiverdata;
@ -41,9 +42,11 @@ static OPLinkReceiverData oplinkreceiverdata;
/* Provide a RCVR driver */
static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel);
static void PIOS_oplinkrcvr_Supervisor(uint32_t ppm_id);
static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id);
const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver = {
.read = PIOS_OPLinkRCVR_Get,
.read = PIOS_OPLinkRCVR_Get,
.get_quality = PIOS_OPLinkRCVR_Quality_Get
};
/* Local Variables */
@ -186,6 +189,16 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
oplinkrcvr_dev->Fresh = false;
}
static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id)
{
uint8_t oplink_quality;
OPLinkStatusLinkQualityGet(&oplink_quality);
/* link_status is in the range 0-128, so scale to a % */
return oplink_quality * 100 / 128;
}
#endif /* PIOS_INCLUDE_OPLINKRCVR */
/**

View File

@ -113,6 +113,34 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}
/**
* @brief Reads input quality from the appropriate driver
* @param[in] rcvr_id driver to read from
* @returns received signal quality expressed as a %
* @retval PIOS_RCVR_NODRIVER driver was not initialized
*/
uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
{
if (rcvr_id == 0) {
return PIOS_RCVR_NODRIVER;
}
struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
/* As no receiver is available assume min */
return 0;
}
if (!rcvr_dev->driver->get_quality) {
/* If no quality is available assume max */
return 100;
}
return rcvr_dev->driver->get_quality(rcvr_dev->lower_id);
}
/**
* @brief Get a semaphore that signals when a new sample is available.
* @param[in] rcvr_id driver to read from

View File

@ -32,6 +32,10 @@
#ifdef PIOS_INCLUDE_SBUS
// Define to report number of frames since last dropped instead of weighted ave
#undef SBUS_GOOD_FRAME_COUNT
#include <uavobjectmanager.h>
#include "pios_sbus_priv.h"
/* Forward Declarations */
@ -42,11 +46,12 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
uint16_t *headroom,
bool *need_yield);
static void PIOS_SBus_Supervisor(uint32_t sbus_id);
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
};
enum pios_sbus_dev_magic {
@ -60,8 +65,17 @@ struct pios_sbus_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
float quality;
#ifdef SBUS_GOOD_FRAME_COUNT
uint8_t frame_count;
#endif /* SBUS_GOOD_FRAME_COUNT */
};
/* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples
* gives about a 200ms response.
*/
#define SBUS_FL_WEIGHTED_AVE 26
struct pios_sbus_dev {
enum pios_sbus_dev_magic magic;
const struct pios_sbus_cfg *cfg;
@ -120,6 +134,10 @@ 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.0f;
#ifdef SBUS_GOOD_FRAME_COUNT
state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
PIOS_SBus_ResetChannels(state);
}
@ -251,18 +269,42 @@ 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) {
#ifndef SBUS_GOOD_FRAME_COUNT
/* Quality trend is towards 0% by default*/
uint8_t quality_trend = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
/* full frame received */
uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
if (flags & SBUS_FLAG_FL) {
/* frame lost, do not update */
} else if (flags & SBUS_FLAG_FS) {
/* failsafe flag active */
PIOS_SBus_ResetChannels(state);
#ifdef SBUS_GOOD_FRAME_COUNT
state->quality = state->frame_count;
state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
/* data looking good */
PIOS_SBus_UnrollChannels(state);
state->failsafe_timer = 0;
#ifdef SBUS_GOOD_FRAME_COUNT
if (++state->frame_count == 255) {
state->quality = state->frame_count--;
}
#else /* SBUS_GOOD_FRAME_COUNT */
/* Quality trend is towards 100% */
quality_trend = 100;
#endif /* SBUS_GOOD_FRAME_COUNT */
if (flags & SBUS_FLAG_FS) {
/* failsafe flag active */
PIOS_SBus_ResetChannels(state);
} else {
/* data looking good */
PIOS_SBus_UnrollChannels(state);
state->failsafe_timer = 0;
}
}
#ifndef SBUS_GOOD_FRAME_COUNT
/* Present quality as a weighted average of good frames */
state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
quality_trend) / SBUS_FL_WEIGHTED_AVE;
#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
/* discard whole frame */
}
@ -341,6 +383,19 @@ static void PIOS_SBus_Supervisor(uint32_t sbus_id)
}
}
static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
{
struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
bool valid = PIOS_SBus_Validate(sbus_dev);
PIOS_Assert(valid);
struct pios_sbus_state *state = &(sbus_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
#endif /* PIOS_INCLUDE_SBUS */
/**

View File

@ -35,10 +35,12 @@ struct pios_rcvr_driver {
void (*init)(uint32_t id);
int32_t (*read)(uint32_t id, uint8_t channel);
xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel);
uint8_t (*get_quality)(uint32_t id);
};
/* Public Functions */
extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
extern uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id);
extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel);
/*! Define error codes for PIOS_RCVR_Get */

View File

@ -34,9 +34,12 @@
#include "pios_dsm_priv.h"
// *** UNTESTED CODE ***
#undef DSM_LINK_QUALITY
/* Forward Declarations */
static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel);
static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id);
static uint16_t PIOS_DSM_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
@ -46,7 +49,8 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id);
/* Local Variables */
const struct pios_rcvr_driver pios_dsm_rcvr_driver = {
.read = PIOS_DSM_Get,
.read = PIOS_DSM_Get,
.get_quality = PIOS_DSM_Quality_Get
};
enum pios_dsm_dev_magic {
@ -60,12 +64,15 @@ struct pios_dsm_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
#ifdef DSM_LOST_FRAME_COUNTER
uint8_t frames_lost_last;
uint16_t frames_lost;
#endif
float quality;
};
/* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples
* gives about a 200ms response.
*/
#define DSM_FL_WEIGHTED_AVE 18
struct pios_dsm_dev {
enum pios_dsm_dev_magic magic;
const struct pios_dsm_cfg *cfg;
@ -164,10 +171,8 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
#ifdef DSM_LOST_FRAME_COUNTER
state->quality = 0.0f;
state->frames_lost_last = 0;
state->frames_lost = 0;
#endif
PIOS_DSM_ResetChannels(dsm_dev);
}
@ -183,13 +188,24 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
static uint8_t resolution = 11;
uint32_t channel_log = 0;
#ifdef DSM_LOST_FRAME_COUNTER
// *** UNTESTED CODE ***
#ifdef DSM_LINK_QUALITY
/* increment the lost frame counter */
uint8_t frames_lost = state->received_data[0];
state->frames_lost += (frames_lost - state->frames_lost_last);
state->frames_lost_last = frames_lost;
#endif
/* We only get a lost frame count when the next good frame comes in */
/* Present quality as a weighted average of good frames */
/* First consider the bad frames */
for (int i = 0; i < frames_lost - state->frames_lost_last; i++) {
state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) /
DSM_FL_WEIGHTED_AVE;
}
/* And now the good frame */
state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) +
100) / DSM_FL_WEIGHTED_AVE;
state->frames_lost_last = frames_lost;
#endif /* DSM_LINK_QUALITY */
/* unroll channels */
uint8_t *s = &(state->received_data[2]);
@ -238,11 +254,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
}
}
#ifdef DSM_LOST_FRAME_COUNTER
/* put lost frames counter into the last channel for debugging */
state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost;
#endif
/* all channels processed */
return 0;
@ -406,6 +417,19 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id)
}
}
static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id)
{
struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id;
bool valid = PIOS_DSM_Validate(dsm_dev);
PIOS_Assert(valid);
struct pios_dsm_state *state = &(dsm_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
#endif /* PIOS_INCLUDE_DSM */
/**

View File

@ -34,12 +34,16 @@
#include "pios_dsm_priv.h"
// *** UNTESTED CODE ***
#undef DSM_LINK_QUALITY
#ifndef PIOS_INCLUDE_RTC
#error PIOS_INCLUDE_RTC must be used to use DSM
#endif
/* Forward Declarations */
static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel);
static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id);
static uint16_t PIOS_DSM_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
@ -49,7 +53,8 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id);
/* Local Variables */
const struct pios_rcvr_driver pios_dsm_rcvr_driver = {
.read = PIOS_DSM_Get,
.read = PIOS_DSM_Get,
.get_quality = PIOS_DSM_Quality_Get
};
enum pios_dsm_dev_magic {
@ -63,12 +68,15 @@ struct pios_dsm_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
#ifdef DSM_LOST_FRAME_COUNTER
uint8_t frames_lost_last;
uint16_t frames_lost;
#endif
float quality;
};
/* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples
* gives about a 200ms response.
*/
#define DSM_FL_WEIGHTED_AVE 18
struct pios_dsm_dev {
enum pios_dsm_dev_magic magic;
const struct pios_dsm_cfg *cfg;
@ -167,10 +175,8 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
#ifdef DSM_LOST_FRAME_COUNTER
state->quality = 0.0f;
state->frames_lost_last = 0;
state->frames_lost = 0;
#endif
PIOS_DSM_ResetChannels(dsm_dev);
}
@ -186,12 +192,24 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
static uint8_t resolution = 11;
uint32_t channel_log = 0;
#ifdef DSM_LOST_FRAME_COUNTER
// *** UNTESTED CODE ***
#ifdef DSM_LINK_QUALITY
/* increment the lost frame counter */
uint8_t frames_lost = state->received_data[0];
state->frames_lost += (frames_lost - state->frames_lost_last);
/* We only get a lost frame count when the next good frame comes in */
/* Present quality as a weighted average of good frames */
/* First consider the bad frames */
for (int i = 0; i < frames_lost - state->frames_lost_last; i++) {
state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) /
DSM_FL_WEIGHTED_AVE;
}
/* And now the good frame */
state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) +
100) / DSM_FL_WEIGHTED_AVE;
state->frames_lost_last = frames_lost;
#endif
#endif /* DSM_LINK_QUALITY */
/* unroll channels */
uint8_t *s = &(state->received_data[2]);
@ -240,11 +258,6 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
}
}
#ifdef DSM_LOST_FRAME_COUNTER
/* put lost frames counter into the last channel for debugging */
state->channel_data[PIOS_DSM_NUM_INPUTS - 1] = state->frames_lost;
#endif
/* all channels processed */
return 0;
@ -408,6 +421,19 @@ static void PIOS_DSM_Supervisor(uint32_t dsm_id)
}
}
static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id)
{
struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id;
bool valid = PIOS_DSM_Validate(dsm_dev);
PIOS_Assert(valid);
struct pios_dsm_state *state = &(dsm_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
#endif /* PIOS_INCLUDE_DSM */
/**

View File

@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

View File

@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

View File

@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

View File

@ -117,6 +117,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/hwsettings.h \
$$UAVOBJECT_SYNTHETICS/gcsreceiver.h \
$$UAVOBJECT_SYNTHETICS/receiveractivity.h \
$$UAVOBJECT_SYNTHETICS/receiverstatus.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h \
$$UAVOBJECT_SYNTHETICS/txpidsettings.h \
$$UAVOBJECT_SYNTHETICS/cameradesired.h \
@ -229,6 +230,7 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/hwsettings.cpp \
$$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \
$$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \
$$UAVOBJECT_SYNTHETICS/receiverstatus.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \
$$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
$$UAVOBJECT_SYNTHETICS/cameradesired.cpp \

View File

@ -0,0 +1,11 @@
<xml>
<object name="ReceiverStatus" singleinstance="true" settings="false" category="System" priority="true">
<description>Receiver status.</description>
<field name="Quality" units="%" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readonly" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>