From a60baf281f612d6d26ed328dba6f800cfdba027d Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Thu, 2 Apr 2015 19:09:38 +0100
Subject: [PATCH 01/16] OP-1803 Initial commit
---
shared/uavobjectdefinition/receiverstatus.xml | 11 +++++++++++
1 file changed, 11 insertions(+)
create mode 100644 shared/uavobjectdefinition/receiverstatus.xml
diff --git a/shared/uavobjectdefinition/receiverstatus.xml b/shared/uavobjectdefinition/receiverstatus.xml
new file mode 100644
index 000000000..5a0415be2
--- /dev/null
+++ b/shared/uavobjectdefinition/receiverstatus.xml
@@ -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>
From a0f180d393f0b5adafea64771421afbe305c65e5 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Thu, 2 Apr 2015 19:09:58 +0100
Subject: [PATCH 02/16] OP-1803 Initial commit
---
flight/modules/Receiver/receiver.c | 21 ++++++++
flight/pios/common/pios_rcvr.c | 28 +++++++++++
flight/pios/common/pios_sbus.c | 50 ++++++++++++++++---
flight/pios/inc/pios_rcvr.h | 2 +
.../boards/coptercontrol/firmware/Makefile | 1 +
.../discoveryf4bare/firmware/UAVObjects.inc | 1 +
.../boards/revolution/firmware/UAVObjects.inc | 1 +
.../boards/revoproto/firmware/UAVObjects.inc | 1 +
.../src/plugins/uavobjects/uavobjects.pro | 2 +
9 files changed, 99 insertions(+), 8 deletions(-)
diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c
index 2f42b7eed..16b05c440 100644
--- a/flight/modules/Receiver/receiver.c
+++ b/flight/modules/Receiver/receiver.c
@@ -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
@@ -99,6 +100,7 @@ struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
+ uint8_t quality;
};
static struct rcvr_activity_fsm activity_fsm;
@@ -148,6 +150,18 @@ 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
*/
@@ -243,6 +257,13 @@ 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
diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c
index b947c5142..66720ff5d 100644
--- a/flight/pios/common/pios_rcvr.c
+++ b/flight/pios/common/pios_rcvr.c
@@ -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 Unitless input value
+ * @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) */
+ PIOS_Assert(0);
+ }
+
+ 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);
+}
+
/**
* @brief Get a semaphore that signals when a new sample is available.
* @param[in] rcvr_id driver to read from
diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c
index 69089e9cd..5531626c8 100644
--- a/flight/pios/common/pios_sbus.c
+++ b/flight/pios/common/pios_sbus.c
@@ -32,6 +32,8 @@
#ifdef PIOS_INCLUDE_SBUS
+#include <uavobjectmanager.h>
+#include <sbusstatus.h>
#include "pios_sbus_priv.h"
/* Forward Declarations */
@@ -42,11 +44,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,
+ .get_quality = PIOS_SBus_Quality_Get
};
enum pios_sbus_dev_magic {
@@ -60,8 +63,14 @@ struct pios_sbus_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
+ uint8_t quality;
};
+/* 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 +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;
PIOS_SBus_ResetChannels(state);
}
@@ -251,18 +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;
+
/* 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);
+ /* Quality trend is towards 0% */
+ quality_trend = 0;
} else {
- /* data looking good */
- PIOS_SBus_UnrollChannels(state);
- state->failsafe_timer = 0;
- }
+ /* 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;
} else {
/* discard whole frame */
}
@@ -341,6 +362,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 state->quality;
+}
+
#endif /* PIOS_INCLUDE_SBUS */
/**
diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h
index 74fac086f..c289635d0 100644
--- a/flight/pios/inc/pios_rcvr.h
+++ b/flight/pios/inc/pios_rcvr.h
@@ -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 */
diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile
index d58b57e64..8df60a8bf 100644
--- a/flight/targets/boards/coptercontrol/firmware/Makefile
+++ b/flight/targets/boards/coptercontrol/firmware/Makefile
@@ -122,6 +122,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
+ SRC += $(OPUAVSYNTHDIR)/receiverstatus.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
index 9f4007db8..d87f24086 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
+++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
@@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
+UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc
index 9f4007db8..d87f24086 100644
--- a/flight/targets/boards/revolution/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc
@@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
+UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
index 987266d1f..0d216aba4 100644
--- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
@@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
+UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 94225fc81..69e032ecc 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -112,6 +112,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 \
@@ -219,6 +220,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 \
From 2e5ecd2dce988ed31be7b980dd41b93e1ec9facd Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Thu, 2 Apr 2015 21:41:40 +0100
Subject: [PATCH 03/16] 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 */
}
From 8276aa3a66bf06a1b6b3a24c982b6146cc49d68f Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 3 Apr 2015 09:47:07 +0100
Subject: [PATCH 04/16] OP-1803 Monitor signal quality when armed too
---
flight/modules/Receiver/receiver.c | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c
index bdcc376a8..da26c23be 100644
--- a/flight/modules/Receiver/receiver.c
+++ b/flight/modules/Receiver/receiver.c
@@ -255,6 +255,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);
+
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
From f9f0e1657c5b4a0124c7ddacad036da19e5686a4 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 3 Apr 2015 15:29:24 +0100
Subject: [PATCH 05/16] OP-1803 Add good frame count option for debug
---
flight/pios/common/pios_sbus.c | 29 ++++++++++++++++++++++++++---
1 file changed, 26 insertions(+), 3 deletions(-)
diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c
index 0d6a5e02e..778d91743 100644
--- a/flight/pios/common/pios_sbus.c
+++ b/flight/pios/common/pios_sbus.c
@@ -32,6 +32,9 @@
#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 <sbusstatus.h>
#include "pios_sbus_priv.h"
@@ -64,6 +67,9 @@ struct pios_sbus_state {
uint8_t frame_found;
uint8_t byte_count;
uint8_t 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
@@ -130,6 +136,9 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
state->failsafe_timer = 0;
state->frame_found = 0;
state->quality = 0;
+#ifdef SBUS_GOOD_FRAME_COUNT
+ state->frame_count = 0;
+#endif /* SBUS_GOOD_FRAME_COUNT */
PIOS_SBus_ResetChannels(state);
}
@@ -261,17 +270,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;
+#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 */
- /* Quality trend is towards 0% */
- quality_trend = 0;
+#ifdef SBUS_GOOD_FRAME_COUNT
+ state->quality = state->frame_count;
+ state->frame_count = 0;
+#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
+#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);
@@ -281,9 +302,11 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
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 */
}
From 32b3f0d55de18ad7aa3932cecce2ed10c7f37edd Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 3 Apr 2015 15:45:00 +0100
Subject: [PATCH 06/16] OP-1803 Remove old header reference
---
flight/pios/common/pios_sbus.c | 1 -
1 file changed, 1 deletion(-)
diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c
index 778d91743..9cd2313e8 100644
--- a/flight/pios/common/pios_sbus.c
+++ b/flight/pios/common/pios_sbus.c
@@ -36,7 +36,6 @@
#undef SBUS_GOOD_FRAME_COUNT
#include <uavobjectmanager.h>
-#include <sbusstatus.h>
#include "pios_sbus_priv.h"
/* Forward Declarations */
From b5a3ad3708cecd693059ea5425332da8f3a122a9 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 3 Apr 2015 18:01:04 +0100
Subject: [PATCH 07/16] OP-1803 Use float to avoid rounding error on signal
quality
---
flight/pios/common/pios_sbus.c | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c
index 9cd2313e8..b7d04c7d7 100644
--- a/flight/pios/common/pios_sbus.c
+++ b/flight/pios/common/pios_sbus.c
@@ -65,7 +65,7 @@ struct pios_sbus_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
- uint8_t quality;
+ float quality;
#ifdef SBUS_GOOD_FRAME_COUNT
uint8_t frame_count;
#endif /* SBUS_GOOD_FRAME_COUNT */
@@ -134,7 +134,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.0f;
#ifdef SBUS_GOOD_FRAME_COUNT
state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
@@ -394,7 +394,7 @@ static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
struct pios_sbus_state *state = &(sbus_dev->state);
- return state->quality;
+ return (uint8_t)(state->quality + 0.5f);
}
#endif /* PIOS_INCLUDE_SBUS */
From b7122008c7fa9b89e7cab3ace19c2d90e97b85e6 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 3 Apr 2015 18:30:36 +0100
Subject: [PATCH 08/16] OP-1803 Uncrustify
---
flight/pios/common/pios_rcvr.c | 2 +-
flight/pios/common/pios_sbus.c | 15 +++++++--------
2 files changed, 8 insertions(+), 9 deletions(-)
diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c
index 590e9f213..89a827885 100644
--- a/flight/pios/common/pios_rcvr.c
+++ b/flight/pios/common/pios_rcvr.c
@@ -116,7 +116,7 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
/**
* @brief Reads input quality from the appropriate driver
* @param[in] rcvr_id driver to read from
- * @returns Unitless input value
+ * @returns received signal quality expressed as a %
* @retval PIOS_RCVR_NODRIVER driver was not initialized
*/
uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
diff --git a/flight/pios/common/pios_sbus.c b/flight/pios/common/pios_sbus.c
index b7d04c7d7..e354a5c5f 100644
--- a/flight/pios/common/pios_sbus.c
+++ b/flight/pios/common/pios_sbus.c
@@ -136,7 +136,7 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
state->frame_found = 0;
state->quality = 0.0f;
#ifdef SBUS_GOOD_FRAME_COUNT
- state->frame_count = 0;
+ state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
PIOS_SBus_ResetChannels(state);
}
@@ -270,7 +270,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
} 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*/
+ /* Quality trend is towards 0% by default*/
uint8_t quality_trend = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
@@ -279,15 +279,14 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
if (flags & SBUS_FLAG_FL) {
/* frame lost, do not update */
#ifdef SBUS_GOOD_FRAME_COUNT
- state->quality = state->frame_count;
- state->frame_count = 0;
+ state->quality = state->frame_count;
+ state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
#ifdef SBUS_GOOD_FRAME_COUNT
- if (++state->frame_count == 255)
- {
- state->quality = state->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;
From b93d9e178e9c9ef038ef44c03a463ba786801eeb Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sat, 4 Apr 2015 14:50:08 +0100
Subject: [PATCH 09/16] OP-1803 Max signal quality is 100%
---
flight/pios/common/pios_rcvr.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c
index 89a827885..4ad7046ce 100644
--- a/flight/pios/common/pios_rcvr.c
+++ b/flight/pios/common/pios_rcvr.c
@@ -134,7 +134,7 @@ uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
if (!rcvr_dev->driver->get_quality) {
/* If no quality is available assume max */
- return 255;
+ return 100;
}
return rcvr_dev->driver->get_quality(rcvr_dev->lower_id);
From 73aeb5d65336cc81051ddbfcd8debe3b17fc98dc Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sat, 4 Apr 2015 22:02:54 +0100
Subject: [PATCH 10/16] OP-1803 Report link quality for OPLink
---
flight/pios/common/pios_oplinkrcvr.c | 15 ++++++++++++++-
1 file changed, 14 insertions(+), 1 deletion(-)
diff --git a/flight/pios/common/pios_oplinkrcvr.c b/flight/pios/common/pios_oplinkrcvr.c
index c4579a0b5..5c967c105 100644
--- a/flight/pios/common/pios_oplinkrcvr.c
+++ b/flight/pios/common/pios_oplinkrcvr.c
@@ -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 */
/**
From 7c7b19ff2d0591ba416c61e301846d9bb87a95fb Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sat, 11 Apr 2015 10:10:04 +0100
Subject: [PATCH 11/16] OP-1289 Merge OP-1803 Only create local telemetry
queues/tasks if needed
---
flight/modules/Telemetry/telemetry.c | 134 +++++++++++++++------------
1 file changed, 76 insertions(+), 58 deletions(-)
diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c
index 58532ce78..9d3d1d5e6 100644
--- a/flight/modules/Telemetry/telemetry.c
+++ b/flight/modules/Telemetry/telemetry.c
@@ -157,36 +157,45 @@ static void updateSettings(channelContext *channel);
*/
int32_t TelemetryStart(void)
{
- UAVObjIterate(®isterLocalObject);
+ // Only start the local telemetry tasks if needed
+ if (localPort()) {
+ UAVObjIterate(®isterLocalObject);
+
+ // Listen to objects of interest
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+ GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+ GCSTelemetryStatsConnectQueue(localChannel.queue);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+ // Start telemetry tasks
+ xTaskCreate(telemetryTxTask,
+ "TelTx",
+ STACK_SIZE_TX_BYTES / 4,
+ &localChannel,
+ TASK_PRIORITY_TX,
+ &localChannel.txTaskHandle);
+ PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
+ localChannel.txTaskHandle);
+ xTaskCreate(telemetryRxTask,
+ "TelRx",
+ STACK_SIZE_RX_BYTES / 4,
+ &localChannel,
+ TASK_PRIORITY_RX,
+ &localChannel.rxTaskHandle);
+ PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
+ localChannel.rxTaskHandle);
+ }
+
+ // Start the telemetry tasks associated with Radio/USB
UAVObjIterate(®isterRadioObject);
// Listen to objects of interest
#ifdef PIOS_TELEM_PRIORITY_QUEUE
- GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
- GCSTelemetryStatsConnectQueue(localChannel.queue);
GCSTelemetryStatsConnectQueue(radioChannel.queue);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
- // Start telemetry tasks
- xTaskCreate(telemetryTxTask,
- "TelTx",
- STACK_SIZE_TX_BYTES / 4,
- &localChannel,
- TASK_PRIORITY_TX,
- &localChannel.txTaskHandle);
- PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
- localChannel.txTaskHandle);
- xTaskCreate(telemetryRxTask,
- "TelRx",
- STACK_SIZE_RX_BYTES / 4,
- &localChannel,
- TASK_PRIORITY_RX,
- &localChannel.rxTaskHandle);
- PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
- localChannel.rxTaskHandle);
-
xTaskCreate(telemetryTxTask,
"RadioTx",
STACK_SIZE_RADIO_TX_BYTES / 4,
@@ -207,6 +216,35 @@ int32_t TelemetryStart(void)
return 0;
}
+/* Intialise a telemetry channel */
+void TelemetryInitializeChannel(channelContext *channel)
+{
+ // Create object queues
+ channel->queue = xQueueCreate(MAX_QUEUE_SIZE,
+ sizeof(UAVObjEvent));
+#if defined(PIOS_TELEM_PRIORITY_QUEUE)
+ channel->priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
+ sizeof(UAVObjEvent));
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+
+ // Initialise UAVTalk
+ channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+
+ // Create periodic event that will be used to update the telemetry stats
+ UAVObjEvent ev;
+ memset(&ev, 0, sizeof(UAVObjEvent));
+
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+ EventPeriodicQueueCreate(&ev,
+ channel->priorityQueue,
+ STATS_UPDATE_PERIOD_MS);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+ EventPeriodicQueueCreate(&ev,
+ channel->queue,
+ STATS_UPDATE_PERIOD_MS);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+}
+
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
@@ -214,58 +252,38 @@ int32_t TelemetryStart(void)
*/
int32_t TelemetryInitialize(void)
{
+ HwSettingsInitialize();
+
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
// Initialize vars
- timeOfLastObjectUpdate = 0;
+ timeOfLastObjectUpdate = 0;
- // Create object queues
- localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
- radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
- localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
- radioChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
-#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+ // Reset link stats
+ txErrors = 0;
+ txRetries = 0;
// Set channel port handlers
localChannel.getPort = localPort;
radioChannel.getPort = radioPort;
- HwSettingsInitialize();
+ // Set the local telemetry baud rate
updateSettings(&localChannel);
+ // Only initialise local channel if telemetry port enabled
+ if (localPort()) {
+ // Initialise channel
+ TelemetryInitializeChannel(&localChannel);
+ // Initialise UAVTalk
+ localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+ }
+
+ // Initialise channel
+ TelemetryInitializeChannel(&radioChannel);
// Initialise UAVTalk
- localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData);
- // Create periodic event that will be used to update the telemetry stats
- // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
- txErrors = 0;
- txRetries = 0;
- UAVObjEvent ev;
- memset(&ev, 0, sizeof(UAVObjEvent));
-
-#ifdef PIOS_TELEM_PRIORITY_QUEUE
- EventPeriodicQueueCreate(&ev,
- localChannel.priorityQueue,
- STATS_UPDATE_PERIOD_MS);
- EventPeriodicQueueCreate(&ev,
- radioChannel.priorityQueue,
- STATS_UPDATE_PERIOD_MS);
-#else /* PIOS_TELEM_PRIORITY_QUEUE */
- EventPeriodicQueueCreate(&ev,
- localChannel.queue,
- STATS_UPDATE_PERIOD_MS);
- EventPeriodicQueueCreate(&ev,
- radioChannel.queue,
- STATS_UPDATE_PERIOD_MS);
-#endif /* PIOS_TELEM_PRIORITY_QUEUE */
-
return 0;
}
From d2a69b2d449f0e0935e837d2ea31018c92a83ca8 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sat, 11 Apr 2015 10:10:04 +0100
Subject: [PATCH 12/16] OP-1803 Merge OP-1289 Only create local telemetry
queues/tasks if needed
---
flight/modules/Telemetry/telemetry.c | 134 +++++++++++++++------------
1 file changed, 76 insertions(+), 58 deletions(-)
diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c
index 58532ce78..9d3d1d5e6 100644
--- a/flight/modules/Telemetry/telemetry.c
+++ b/flight/modules/Telemetry/telemetry.c
@@ -157,36 +157,45 @@ static void updateSettings(channelContext *channel);
*/
int32_t TelemetryStart(void)
{
- UAVObjIterate(®isterLocalObject);
+ // Only start the local telemetry tasks if needed
+ if (localPort()) {
+ UAVObjIterate(®isterLocalObject);
+
+ // Listen to objects of interest
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+ GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+ GCSTelemetryStatsConnectQueue(localChannel.queue);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+ // Start telemetry tasks
+ xTaskCreate(telemetryTxTask,
+ "TelTx",
+ STACK_SIZE_TX_BYTES / 4,
+ &localChannel,
+ TASK_PRIORITY_TX,
+ &localChannel.txTaskHandle);
+ PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
+ localChannel.txTaskHandle);
+ xTaskCreate(telemetryRxTask,
+ "TelRx",
+ STACK_SIZE_RX_BYTES / 4,
+ &localChannel,
+ TASK_PRIORITY_RX,
+ &localChannel.rxTaskHandle);
+ PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
+ localChannel.rxTaskHandle);
+ }
+
+ // Start the telemetry tasks associated with Radio/USB
UAVObjIterate(®isterRadioObject);
// Listen to objects of interest
#ifdef PIOS_TELEM_PRIORITY_QUEUE
- GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
- GCSTelemetryStatsConnectQueue(localChannel.queue);
GCSTelemetryStatsConnectQueue(radioChannel.queue);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
- // Start telemetry tasks
- xTaskCreate(telemetryTxTask,
- "TelTx",
- STACK_SIZE_TX_BYTES / 4,
- &localChannel,
- TASK_PRIORITY_TX,
- &localChannel.txTaskHandle);
- PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
- localChannel.txTaskHandle);
- xTaskCreate(telemetryRxTask,
- "TelRx",
- STACK_SIZE_RX_BYTES / 4,
- &localChannel,
- TASK_PRIORITY_RX,
- &localChannel.rxTaskHandle);
- PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
- localChannel.rxTaskHandle);
-
xTaskCreate(telemetryTxTask,
"RadioTx",
STACK_SIZE_RADIO_TX_BYTES / 4,
@@ -207,6 +216,35 @@ int32_t TelemetryStart(void)
return 0;
}
+/* Intialise a telemetry channel */
+void TelemetryInitializeChannel(channelContext *channel)
+{
+ // Create object queues
+ channel->queue = xQueueCreate(MAX_QUEUE_SIZE,
+ sizeof(UAVObjEvent));
+#if defined(PIOS_TELEM_PRIORITY_QUEUE)
+ channel->priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
+ sizeof(UAVObjEvent));
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+
+ // Initialise UAVTalk
+ channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+
+ // Create periodic event that will be used to update the telemetry stats
+ UAVObjEvent ev;
+ memset(&ev, 0, sizeof(UAVObjEvent));
+
+#ifdef PIOS_TELEM_PRIORITY_QUEUE
+ EventPeriodicQueueCreate(&ev,
+ channel->priorityQueue,
+ STATS_UPDATE_PERIOD_MS);
+#else /* PIOS_TELEM_PRIORITY_QUEUE */
+ EventPeriodicQueueCreate(&ev,
+ channel->queue,
+ STATS_UPDATE_PERIOD_MS);
+#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+}
+
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
@@ -214,58 +252,38 @@ int32_t TelemetryStart(void)
*/
int32_t TelemetryInitialize(void)
{
+ HwSettingsInitialize();
+
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
// Initialize vars
- timeOfLastObjectUpdate = 0;
+ timeOfLastObjectUpdate = 0;
- // Create object queues
- localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
- radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
-#if defined(PIOS_TELEM_PRIORITY_QUEUE)
- localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
- radioChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
- sizeof(UAVObjEvent));
-#endif /* PIOS_TELEM_PRIORITY_QUEUE */
+ // Reset link stats
+ txErrors = 0;
+ txRetries = 0;
// Set channel port handlers
localChannel.getPort = localPort;
radioChannel.getPort = radioPort;
- HwSettingsInitialize();
+ // Set the local telemetry baud rate
updateSettings(&localChannel);
+ // Only initialise local channel if telemetry port enabled
+ if (localPort()) {
+ // Initialise channel
+ TelemetryInitializeChannel(&localChannel);
+ // Initialise UAVTalk
+ localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
+ }
+
+ // Initialise channel
+ TelemetryInitializeChannel(&radioChannel);
// Initialise UAVTalk
- localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData);
- // Create periodic event that will be used to update the telemetry stats
- // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
- txErrors = 0;
- txRetries = 0;
- UAVObjEvent ev;
- memset(&ev, 0, sizeof(UAVObjEvent));
-
-#ifdef PIOS_TELEM_PRIORITY_QUEUE
- EventPeriodicQueueCreate(&ev,
- localChannel.priorityQueue,
- STATS_UPDATE_PERIOD_MS);
- EventPeriodicQueueCreate(&ev,
- radioChannel.priorityQueue,
- STATS_UPDATE_PERIOD_MS);
-#else /* PIOS_TELEM_PRIORITY_QUEUE */
- EventPeriodicQueueCreate(&ev,
- localChannel.queue,
- STATS_UPDATE_PERIOD_MS);
- EventPeriodicQueueCreate(&ev,
- radioChannel.queue,
- STATS_UPDATE_PERIOD_MS);
-#endif /* PIOS_TELEM_PRIORITY_QUEUE */
-
return 0;
}
From b87a1c1fa6a73633b7eb6d1773e473bc6370f514 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sun, 19 Apr 2015 18:41:04 +0100
Subject: [PATCH 13/16] OP-1803 Enable DSM link quality so testers have
something to work with
---
flight/pios/stm32f10x/pios_dsm.c | 56 +++++++++++++++++++++++---------
flight/pios/stm32f4xx/pios_dsm.c | 56 +++++++++++++++++++++++---------
2 files changed, 81 insertions(+), 31 deletions(-)
diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c
index 0a3450a9f..a058ee9c2 100644
--- a/flight/pios/stm32f10x/pios_dsm.c
+++ b/flight/pios/stm32f10x/pios_dsm.c
@@ -34,9 +34,12 @@
#include "pios_dsm_priv.h"
+// *** UNTESTED CODE ***
+#define 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 */
/**
diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c
index 055f9505c..764bc231b 100644
--- a/flight/pios/stm32f4xx/pios_dsm.c
+++ b/flight/pios/stm32f4xx/pios_dsm.c
@@ -34,12 +34,16 @@
#include "pios_dsm_priv.h"
+// *** UNTESTED CODE ***
+#define 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 */
/**
From d28831008f4b49ad65c88bd872eae33c618b3e7e Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Mon, 20 Apr 2015 21:40:42 +0100
Subject: [PATCH 14/16] OP-1803 Return link quality of zero rather than throw
an exception if no receiver if the receiver port is undefined
---
flight/pios/common/pios_rcvr.c | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c
index 4ad7046ce..afae44b2a 100644
--- a/flight/pios/common/pios_rcvr.c
+++ b/flight/pios/common/pios_rcvr.c
@@ -129,7 +129,8 @@ uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
- PIOS_Assert(0);
+ /* As no receiver is available assume min */
+ return 0;
}
if (!rcvr_dev->driver->get_quality) {
From 8689b2d0caab34124617b8bf82596530be9b95ba Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 8 May 2015 07:38:12 +0100
Subject: [PATCH 15/16] OP-1803 Don't report error codes as %
---
flight/modules/Receiver/receiver.c | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c
index da26c23be..16f1c95d5 100644
--- a/flight/modules/Receiver/receiver.c
+++ b/flight/modules/Receiver/receiver.c
@@ -653,14 +653,19 @@ static bool updateRcvrStatus(
{
extern uint32_t pios_rcvr_group_map[];
bool activity_updated = false;
- uint8_t quality;
+ 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(&quality);
+ ReceiverStatusQualitySet(&fsm->quality);
activity_updated = true;
}
From 023f157ba89748472b854f685336a1366ae2f1a9 Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Fri, 8 May 2015 07:42:32 +0100
Subject: [PATCH 16/16] OP-1803 Disable untested DSM link quality code
---
flight/pios/stm32f10x/pios_dsm.c | 2 +-
flight/pios/stm32f4xx/pios_dsm.c | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/flight/pios/stm32f10x/pios_dsm.c b/flight/pios/stm32f10x/pios_dsm.c
index a058ee9c2..44fa484be 100644
--- a/flight/pios/stm32f10x/pios_dsm.c
+++ b/flight/pios/stm32f10x/pios_dsm.c
@@ -35,7 +35,7 @@
#include "pios_dsm_priv.h"
// *** UNTESTED CODE ***
-#define DSM_LINK_QUALITY
+#undef DSM_LINK_QUALITY
/* Forward Declarations */
static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel);
diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c
index 764bc231b..bdd8244e1 100644
--- a/flight/pios/stm32f4xx/pios_dsm.c
+++ b/flight/pios/stm32f4xx/pios_dsm.c
@@ -35,7 +35,7 @@
#include "pios_dsm_priv.h"
// *** UNTESTED CODE ***
-#define DSM_LINK_QUALITY
+#undef DSM_LINK_QUALITY
#ifndef PIOS_INCLUDE_RTC
#error PIOS_INCLUDE_RTC must be used to use DSM