mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-30 15:52:12 +01:00
Merge branch 'steve/OP-1803_s.bus_dropped_frames' into next
This commit is contained in:
commit
5b11dbbf67
@ -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.
|
||||
*/
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -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
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -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 */
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
|
||||
UAVOBJSRCFILENAMES += flightstatus
|
||||
UAVOBJSRCFILENAMES += hwsettings
|
||||
UAVOBJSRCFILENAMES += receiveractivity
|
||||
UAVOBJSRCFILENAMES += receiverstatus
|
||||
UAVOBJSRCFILENAMES += cameradesired
|
||||
UAVOBJSRCFILENAMES += camerastabsettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
|
@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
|
||||
UAVOBJSRCFILENAMES += flightstatus
|
||||
UAVOBJSRCFILENAMES += hwsettings
|
||||
UAVOBJSRCFILENAMES += receiveractivity
|
||||
UAVOBJSRCFILENAMES += receiverstatus
|
||||
UAVOBJSRCFILENAMES += cameradesired
|
||||
UAVOBJSRCFILENAMES += camerastabsettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
|
@ -108,6 +108,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
|
||||
UAVOBJSRCFILENAMES += flightstatus
|
||||
UAVOBJSRCFILENAMES += hwsettings
|
||||
UAVOBJSRCFILENAMES += receiveractivity
|
||||
UAVOBJSRCFILENAMES += receiverstatus
|
||||
UAVOBJSRCFILENAMES += cameradesired
|
||||
UAVOBJSRCFILENAMES += camerastabsettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
|
@ -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 \
|
||||
|
11
shared/uavobjectdefinition/receiverstatus.xml
Normal file
11
shared/uavobjectdefinition/receiverstatus.xml
Normal 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>
|
Loading…
x
Reference in New Issue
Block a user