diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 1f38ba51e7..3c39418d01 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1159,7 +1159,7 @@ const clivalue_t valueTable[] = {
{ "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_alarm) },
#ifdef USE_RX_LINK_QUALITY_INFO
- { "osd_link_quality_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, link_quality_alarm) },
+ { "osd_link_quality_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 300 }, PG_OSD_CONFIG, offsetof(osdConfig_t, link_quality_alarm) },
#endif
{ "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) },
{ "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) },
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index d5145ba2eb..76018e781f 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -156,6 +156,7 @@ CMS_Menu menuOsdActiveElems = {
};
static uint8_t osdConfig_rssi_alarm;
+static uint16_t osdConfig_link_quality_alarm;
static uint16_t osdConfig_cap_alarm;
static uint16_t osdConfig_alt_alarm;
static uint8_t batteryConfig_vbatDurationForWarning;
@@ -164,6 +165,7 @@ static uint8_t batteryConfig_vbatDurationForCritical;
static long menuAlarmsOnEnter(void)
{
osdConfig_rssi_alarm = osdConfig()->rssi_alarm;
+ osdConfig_link_quality_alarm = osdConfig()->link_quality_alarm;
osdConfig_cap_alarm = osdConfig()->cap_alarm;
osdConfig_alt_alarm = osdConfig()->alt_alarm;
batteryConfig_vbatDurationForWarning = batteryConfig()->vbatDurationForWarning;
@@ -177,6 +179,7 @@ static long menuAlarmsOnExit(const OSD_Entry *self)
UNUSED(self);
osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm;
+ osdConfigMutable()->link_quality_alarm = osdConfig_link_quality_alarm;
osdConfigMutable()->cap_alarm = osdConfig_cap_alarm;
osdConfigMutable()->alt_alarm = osdConfig_alt_alarm;
batteryConfigMutable()->vbatDurationForWarning = batteryConfig_vbatDurationForWarning;
@@ -189,6 +192,7 @@ const OSD_Entry menuAlarmsEntries[] =
{
{"--- ALARMS ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0},
+ {"LINK QUALITY", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_link_quality_alarm, 5, 300, 5}, 0},
{"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0},
{"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0},
{"VBAT WARN DUR", OME_UINT8, NULL, &(OSD_UINT8_t){ &batteryConfig_vbatDurationForWarning, 0, 200, 1 }, 0 },
diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c
index ee881f42c5..6019d8cf6b 100644
--- a/src/main/osd/osd.c
+++ b/src/main/osd/osd.c
@@ -341,7 +341,7 @@ static void osdResetStats(void)
stats.max_g_force = 0;
stats.max_esc_temp = 0;
stats.max_esc_rpm = 0;
- stats.min_link_quality = 99; // percent
+ stats.min_link_quality = (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) ? 300 : 99; // CRSF : percent
}
static void osdUpdateStats(void)
@@ -613,7 +613,7 @@ static uint8_t osdShowStats(uint16_t endBatteryVoltage, int statsRowCount)
#ifdef USE_RX_LINK_QUALITY_INFO
if (osdStatGetState(OSD_STAT_MIN_LINK_QUALITY)) {
- itoa(stats.min_link_quality, buff, 10);
+ tfp_sprintf(buff, "%d", stats.min_link_quality);
strcat(buff, "%");
osdDisplayStatisticLabel(top++, "MIN LINK", buff);
}
diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h
index 6bcf0eb745..a6022fa568 100644
--- a/src/main/osd/osd.h
+++ b/src/main/osd/osd.h
@@ -249,7 +249,7 @@ typedef struct osdConfig_s {
uint8_t ahInvert; // invert the artificial horizon
uint8_t osdProfileIndex;
uint8_t overlay_radio_mode;
- uint8_t link_quality_alarm;
+ uint16_t link_quality_alarm;
} osdConfig_t;
PG_DECLARE(osdConfig_t, osdConfig);
@@ -265,7 +265,7 @@ typedef struct statistic_s {
float max_g_force;
int16_t max_esc_temp;
int32_t max_esc_rpm;
- uint8_t min_link_quality;
+ uint16_t min_link_quality;
} statistic_t;
extern timeUs_t resumeRefreshAt;
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 26aad33523..6bd65c1668 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -835,13 +835,17 @@ static void osdElementHorizonSidebars(osdElementParms_t *element)
#ifdef USE_RX_LINK_QUALITY_INFO
static void osdElementLinkQuality(osdElementParms_t *element)
{
- // change range to 0-9 (two sig. fig. adds little extra value, also reduces screen estate)
- uint8_t osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
- if (osdLinkQuality >= 10) {
- osdLinkQuality = 9;
+ uint16_t osdLinkQuality = 0;
+ if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-300
+ osdLinkQuality = rxGetLinkQuality() / 3.41;
+ tfp_sprintf(element->buff, "%3d", osdLinkQuality);
+ } else { // 0-9
+ osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
+ if (osdLinkQuality >= 10) {
+ osdLinkQuality = 9;
+ }
+ tfp_sprintf(element->buff, "%1d", osdLinkQuality);
}
-
- tfp_sprintf(element->buff, "%1d", osdLinkQuality);
}
#endif // USE_RX_LINK_QUALITY_INFO
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 57b580bbd7..2e2a912767 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -149,6 +149,11 @@ typedef struct crsfPayloadLinkstatistics_s {
static timeUs_t lastLinkStatisticsFrameUs;
+#ifdef USE_RX_LINK_QUALITY_INFO
+STATIC_UNIT_TESTED uint16_t scaleCrsfLq(uint16_t lqvalue) {
+ return (lqvalue % 100) ? ((lqvalue * 3.41) + 1) : (lqvalue * 3.41);
+}
+#endif
static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
{
const crsfLinkStatistics_t stats = *statsPtr;
@@ -159,6 +164,12 @@ static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr,
setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
}
+#ifdef USE_RX_LINK_QUALITY_INFO
+ if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
+ setLinkQualityDirect(scaleCrsfLq((stats.rf_Mode * 100) + stats.uplink_Link_quality));
+ }
+#endif
+
switch (debugMode) {
case DEBUG_CRSF_LINK_STATISTICS_UPLINK:
DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
@@ -188,6 +199,11 @@ static void crsfCheckRssi(uint32_t currentTimeUs) {
if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL_CRSF);
}
+#ifdef USE_RX_LINK_QUALITY_INFO
+ if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
+ setLinkQualityDirect(0);
+ }
+#endif
}
}
#endif
@@ -377,6 +393,11 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
if (rssiSource == RSSI_SOURCE_NONE) {
rssiSource = RSSI_SOURCE_RX_PROTOCOL_CRSF;
}
+#ifdef USE_RX_LINK_QUALITY_INFO
+ if (linkQualitySource == LQ_SOURCE_NONE) {
+ linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
+ }
+#endif
return serialPort != NULL;
}
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index f3e64fdeee..d5e4a7098d 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -77,7 +77,7 @@ static timeUs_t lastMspRssiUpdateUs = 0;
static pt1Filter_t frameErrFilter;
#ifdef USE_RX_LINK_QUALITY_INFO
-static uint8_t linkQuality = 0;
+static uint16_t linkQuality = 0;
#endif
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
@@ -86,6 +86,7 @@ static uint8_t linkQuality = 0;
#define RSSI_OFFSET_SCALING (1024 / 100.0f)
rssiSource_e rssiSource;
+linkQualitySource_e linkQualitySource;
static bool rxDataProcessingRequired = false;
static bool auxiliaryProcessingRequired = false;
@@ -358,11 +359,11 @@ void resumeRxPwmPpmSignal(void)
#ifdef USE_RX_LINK_QUALITY_INFO
#define LINK_QUALITY_SAMPLE_COUNT 16
-static uint8_t updateLinkQualitySamples(uint8_t value)
+STATIC_UNIT_TESTED uint16_t updateLinkQualitySamples(uint16_t value)
{
- static uint8_t samples[LINK_QUALITY_SAMPLE_COUNT];
+ static uint16_t samples[LINK_QUALITY_SAMPLE_COUNT];
static uint8_t sampleIndex = 0;
- static unsigned sum = 0;
+ static uint16_t sum = 0;
sum += value - samples[sampleIndex];
samples[sampleIndex] = value;
@@ -374,8 +375,10 @@ static uint8_t updateLinkQualitySamples(uint8_t value)
static void setLinkQuality(bool validFrame, timeDelta_t currentDeltaTime)
{
#ifdef USE_RX_LINK_QUALITY_INFO
- // calculate new sample mean
- linkQuality = updateLinkQualitySamples(validFrame ? LINK_QUALITY_MAX_VALUE : 0);
+ if (linkQualitySource != LQ_SOURCE_RX_PROTOCOL_CRSF) {
+ // calculate new sample mean
+ linkQuality = updateLinkQualitySamples(validFrame ? LINK_QUALITY_MAX_VALUE : 0);
+ }
#endif
if (rssiSource == RSSI_SOURCE_FRAME_ERRORS) {
@@ -396,6 +399,15 @@ static void setLinkQuality(bool validFrame, timeDelta_t currentDeltaTime)
}
}
+void setLinkQualityDirect(uint16_t linkqualityValue)
+{
+#ifdef USE_RX_LINK_QUALITY_INFO
+ linkQuality = linkqualityValue;
+#else
+ UNUSED(linkqualityValue);
+#endif
+}
+
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
{
bool signalReceived = false;
@@ -746,14 +758,14 @@ uint8_t getRssiPercent(void)
}
#ifdef USE_RX_LINK_QUALITY_INFO
-uint8_t rxGetLinkQuality(void)
+uint16_t rxGetLinkQuality(void)
{
return linkQuality;
}
-uint8_t rxGetLinkQualityPercent(void)
+uint16_t rxGetLinkQualityPercent(void)
{
- return scaleRange(rxGetLinkQuality(), 0, LINK_QUALITY_MAX_VALUE, 0, 100);
+ return (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) ? (linkQuality / 3.41) : scaleRange(linkQuality, 0, LINK_QUALITY_MAX_VALUE, 0, 100);
}
#endif
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 923dd6d380..051f0080c3 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -148,6 +148,13 @@ typedef enum {
extern rssiSource_e rssiSource;
+typedef enum {
+ LQ_SOURCE_NONE = 0,
+ LQ_SOURCE_RX_PROTOCOL_CRSF,
+} linkQualitySource_e;
+
+extern linkQualitySource_e linkQualitySource;
+
extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
void rxInit(void);
@@ -170,10 +177,11 @@ uint16_t getRssi(void);
uint8_t getRssiPercent(void);
bool isRssiConfigured(void);
-#define LINK_QUALITY_MAX_VALUE 255
+#define LINK_QUALITY_MAX_VALUE 1023
-uint8_t rxGetLinkQuality(void);
-uint8_t rxGetLinkQualityPercent(void);
+uint16_t rxGetLinkQuality(void);
+void setLinkQualityDirect(uint16_t linkqualityValue);
+uint16_t rxGetLinkQualityPercent(void);
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);
diff --git a/src/test/Makefile b/src/test/Makefile
index 70413bed7b..f66c884fc1 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -187,6 +187,29 @@ osd_unittest_DEFINES := \
USE_RTC_TIME= \
USE_ADC_INTERNAL=
+link_quality_unittest_SRC := \
+ $(USER_DIR)/osd/osd.c \
+ $(USER_DIR)/osd/osd_elements.c \
+ $(USER_DIR)/common/typeconversion.c \
+ $(USER_DIR)/drivers/display.c \
+ $(USER_DIR)/drivers/serial.c \
+ $(USER_DIR)/common/crc.c \
+ $(USER_DIR)/common/maths.c \
+ $(USER_DIR)/common/printf.c \
+ $(USER_DIR)/common/streambuf.c \
+ $(USER_DIR)/common/time.c \
+ $(USER_DIR)/fc/runtime_config.c \
+ $(USER_DIR)/common/bitarray.c \
+ $(USER_DIR)/fc/rc_modes.c \
+ $(USER_DIR)/rx/rx.c \
+ $(USER_DIR)/pg/pg.c \
+ $(USER_DIR)/rx/crsf.c \
+ $(USER_DIR)/pg/rx.c
+
+link_quality_unittest_DEFINES := \
+ USE_OSD= \
+ USE_CRSF_LINK_STATISTICS= \
+ USE_RX_LINK_QUALITY_INFO=
pg_unittest_SRC := \
$(USER_DIR)/pg/pg.c
diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc
new file mode 100644
index 0000000000..ffcd7d669a
--- /dev/null
+++ b/src/test/unit/link_quality_unittest.cc
@@ -0,0 +1,545 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+extern "C" {
+ #include "platform.h"
+ #include "build/debug.h"
+
+ #include "blackbox/blackbox.h"
+ #include "blackbox/blackbox_io.h"
+
+ #include "common/time.h"
+ #include "common/crc.h"
+ #include "common/utils.h"
+ #include "common/printf.h"
+ #include "common/streambuf.h"
+
+ #include "drivers/max7456_symbols.h"
+ #include "drivers/persistent.h"
+ #include "drivers/serial.h"
+ #include "drivers/system.h"
+
+ #include "fc/config.h"
+ #include "fc/core.h"
+ #include "fc/rc_controls.h"
+ #include "fc/rc_modes.h"
+ #include "fc/runtime_config.h"
+
+ #include "flight/mixer.h"
+ #include "flight/pid.h"
+ #include "flight/imu.h"
+
+ #include "io/beeper.h"
+ #include "io/gps.h"
+ #include "io/serial.h"
+
+ #include "osd/osd.h"
+ #include "osd/osd_elements.h"
+
+ #include "pg/pg.h"
+ #include "pg/pg_ids.h"
+ #include "pg/rx.h"
+
+ #include "rx/rx.h"
+
+ #include "sensors/battery.h"
+
+ attitudeEulerAngles_t attitude;
+ pidProfile_t *currentPidProfile;
+ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+ uint8_t GPS_numSat;
+ uint16_t GPS_distanceToHome;
+ int16_t GPS_directionToHome;
+ uint32_t GPS_distanceFlownInCm;
+ int32_t GPS_coord[2];
+ gpsSolutionData_t gpsSol;
+ float motor[8];
+ float motorOutputHigh = 2047;
+ float motorOutputLow = 1000;
+ acc_t acc;
+ float accAverage[XYZ_AXIS_COUNT];
+
+ PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
+ PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
+ PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+ PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0);
+ PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
+
+ timeUs_t simulationTime = 0;
+
+ void osdRefresh(timeUs_t currentTimeUs);
+ uint16_t updateLinkQualitySamples(uint16_t value);
+ uint16_t scaleCrsfLq(uint16_t lqvalue);
+#define LINK_QUALITY_SAMPLE_COUNT 16
+}
+
+/* #define DEBUG_OSD */
+
+#include "unittest_macros.h"
+#include "unittest_displayport.h"
+#include "gtest/gtest.h"
+
+extern "C" {
+ PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
+
+ boxBitmask_t rcModeActivationMask;
+ int16_t debug[DEBUG16_VALUE_COUNT];
+ uint8_t debugMode = 0;
+
+ uint16_t updateLinkQualitySamples(uint16_t value);
+
+ extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range);
+}
+void setDefaultSimulationState()
+{
+
+ setLinkQualityDirect(LINK_QUALITY_MAX_VALUE);
+
+}
+/*
+ * Performs a test of the OSD actions on arming.
+ * (reused throughout the test suite)
+ */
+void doTestArm(bool testEmpty = true)
+{
+ // given
+ // craft has been armed
+ ENABLE_ARMING_FLAG(ARMED);
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+
+ // then
+ // arming alert displayed
+ displayPortTestBufferSubstring(12, 7, "ARMED");
+
+ // given
+ // armed alert times out (0.5 seconds)
+ simulationTime += 0.5e6;
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+
+ // then
+ // arming alert disappears
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+ if (testEmpty) {
+ displayPortTestBufferIsEmpty();
+ }
+}
+
+/*
+ * Auxiliary function. Test is there're stats that must be shown
+ */
+bool isSomeStatEnabled(void) {
+ return (osdConfigMutable()->enabled_stats != 0);
+}
+
+/*
+ * Performs a test of the OSD actions on disarming.
+ * (reused throughout the test suite)
+ */
+void doTestDisarm()
+{
+ // given
+ // craft is disarmed after having been armed
+ DISABLE_ARMING_FLAG(ARMED);
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+
+ // then
+ // post flight statistics displayed
+ if (isSomeStatEnabled()) {
+ displayPortTestBufferSubstring(2, 2, " --- STATS ---");
+ }
+}
+
+/*
+ * Tests initialisation of the OSD and the power on splash screen.
+ */
+TEST(LQTest, TestInit)
+{
+ // given
+ // display port is initialised
+ displayPortTestInit();
+
+ // and
+ // default state values are set
+ setDefaultSimulationState();
+
+ // and
+ // this battery configuration (used for battery voltage elements)
+ batteryConfigMutable()->vbatmincellvoltage = 330;
+ batteryConfigMutable()->vbatmaxcellvoltage = 430;
+
+ // when
+ // OSD is initialised
+ osdInit(&testDisplayPort);
+
+ // then
+ // display buffer should contain splash screen
+ displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
+ displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
+ displayPortTestBufferSubstring(11, 10, "+ PITCH UP");
+
+ // when
+ // splash screen timeout has elapsed
+ simulationTime += 4e6;
+ osdUpdate(simulationTime);
+
+ // then
+ // display buffer should be empty
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+ displayPortTestBufferIsEmpty();
+}
+/*
+ * Tests the Tests the OSD_LINK_QUALITY element updateLinkQualitySamples default LQ_SOURCE_NONE
+ */
+TEST(LQTest, TestElement_LQ_SOURCE_NONE_SAMPLES)
+{
+ // given
+
+
+ linkQualitySource = LQ_SOURCE_NONE;
+
+ osdConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
+ osdConfigMutable()->link_quality_alarm = 0;
+
+ osdAnalyzeActiveElements();
+
+ // when samples populated 100%
+ for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
+ setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
+ }
+
+
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // then
+ displayPortTestBufferSubstring(8, 1, "9");
+
+ // when updateLinkQualitySamples used 50% rounds to 4
+ for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
+ setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
+ setLinkQualityDirect(updateLinkQualitySamples(0));
+ }
+
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // then
+ displayPortTestBufferSubstring(8, 1, "4");
+
+}
+/*
+ * Tests the Tests the OSD_LINK_QUALITY element values default LQ_SOURCE_NONE
+ */
+TEST(LQTest, TestElement_LQ_SOURCE_NONE_VALUES)
+{
+ // given
+
+
+ linkQualitySource = LQ_SOURCE_NONE;
+
+ osdConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
+ osdConfigMutable()->link_quality_alarm = 0;
+
+ osdAnalyzeActiveElements();
+ // when LINK_QUALITY_MAX_VALUE to 1 by 10%
+ uint16_t testscale = 0;
+ for (int testdigit = 10; testdigit > 0; testdigit--) {
+ testscale = testdigit * 102.3;
+ setLinkQualityDirect(testscale);
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+#ifdef DEBUG_OSD
+ printf("%d %d\n",testscale, testdigit);
+ displayPortTestPrint();
+#endif
+ // then
+ if (testdigit >= 10){
+ displayPortTestBufferSubstring(7, 1," 9");
+ }else{
+ displayPortTestBufferSubstring(7, 1," %1d", testdigit - 1);
+ }
+ }
+}
+/*
+ * Tests the OSD_LINK_QUALITY element LQ RX_PROTOCOL_CRSF.
+ */
+TEST(LQTest, TestElementLQ_PROTOCOL_CRSF_VALUES)
+{
+ // given
+ linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
+
+ osdConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
+ osdConfigMutable()->link_quality_alarm = 0;
+
+ osdAnalyzeActiveElements();
+
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // crsf setLinkQualityDirect 0-300;
+
+ for (uint16_t x = 0; x <= 300; x++) {
+ // when x scaled
+ setLinkQualityDirect(scaleCrsfLq(x));
+ // then rxGetLinkQuality Osd should be x
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+ displayPortTestBufferSubstring(8, 1,"%3d", x);
+
+ }
+}
+/*
+ * Tests the LQ Alarms
+ *
+*/
+TEST(LQTest, TestLQAlarm)
+{
+ // given
+ // default state is set
+ setDefaultSimulationState();
+
+ linkQualitySource = LQ_SOURCE_NONE;
+
+ // and
+ // the following OSD elements are visible
+
+ osdConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
+
+ // and
+ // this set of alarm values
+
+ osdConfigMutable()->link_quality_alarm = 80;
+ stateFlags |= GPS_FIX | GPS_FIX_HOME;
+
+ osdAnalyzeActiveElements();
+
+ // and
+ // using the metric unit system
+ osdConfigMutable()->units = OSD_UNIT_METRIC;
+
+ // when
+ // the craft is armed
+ doTestArm(false);
+
+ for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
+ setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
+ }
+
+ // then
+ // no elements should flash as all values are out of alarm range
+ for (int i = 0; i < 30; i++) {
+ // Check for visibility every 100ms, elements should always be visible
+ simulationTime += 0.1e6;
+ osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+ printf("%d\n", i);
+#endif
+ displayPortTestBufferSubstring(8, 1, "9");
+
+ }
+
+ setLinkQualityDirect(512);
+ simulationTime += 60e6;
+ osdRefresh(simulationTime);
+
+ // then
+ // elements showing values in alarm range should flash
+ for (int i = 0; i < 15; i++) {
+ // Blinking should happen at 5Hz
+ simulationTime += 0.2e6;
+ osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+ printf("%d\n", i);
+ displayPortTestPrint();
+#endif
+ if (i % 2 == 0) {
+ displayPortTestBufferSubstring(8, 1, "5");
+ } else {
+ displayPortTestBufferIsEmpty();
+ }
+ }
+
+ doTestDisarm();
+ simulationTime += 60e6;
+ osdRefresh(simulationTime);
+}
+
+// STUBS
+extern "C" {
+
+ uint32_t micros() {
+ return simulationTime;
+ }
+
+ uint32_t millis() {
+ return micros() / 1000;
+ }
+
+ bool featureIsEnabled(uint32_t) { return true; }
+ void beeperConfirmationBeeps(uint8_t) {}
+ bool isBeeperOn() { return false; }
+ uint8_t getCurrentPidProfileIndex() { return 0; }
+ uint8_t getCurrentControlRateProfileIndex() { return 0; }
+ batteryState_e getBatteryState() { return BATTERY_OK; }
+ uint8_t getBatteryCellCount() { return 4; }
+ uint16_t getBatteryVoltage() { return 1680; }
+ uint16_t getBatteryAverageCellVoltage() { return 420; }
+ int32_t getAmperage() { return 0; }
+ int32_t getMAhDrawn() { return 0; }
+ int32_t getEstimatedAltitudeCm() { return 0; }
+ int32_t getEstimatedVario() { return 0; }
+ unsigned int blackboxGetLogNumber() { return 0; }
+ bool isBlackboxDeviceWorking() { return true; }
+ bool isBlackboxDeviceFull() { return false; }
+ serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
+ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
+ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
+ bool cmsDisplayPortRegister(displayPort_t *) { return false; }
+ uint16_t getCoreTemperatureCelsius(void) { return 0; }
+ bool isFlipOverAfterCrashActive(void) { return false; }
+ float pidItermAccelerator(void) { return 1.0; }
+ uint8_t getMotorCount(void){ return 4; }
+ bool areMotorsRunning(void){ return true; }
+ bool pidOsdAntiGravityActive(void) { return false; }
+ bool failsafeIsActive(void) { return false; }
+ bool gpsRescueIsConfigured(void) { return false; }
+ int8_t calculateThrottlePercent(void) { return 0; }
+ uint32_t persistentObjectRead(persistentObjectId_e) { return 0; }
+ void persistentObjectWrite(persistentObjectId_e, uint32_t) {}
+ void failsafeOnRxSuspend(uint32_t ) {}
+ void failsafeOnRxResume(void) {}
+ void featureDisable(uint32_t) { }
+ bool rxMspFrameComplete(void) { return false; }
+ bool isPPMDataBeingReceived(void) { return false; }
+ bool isPWMDataBeingReceived(void) { return false; }
+ void resetPPMDataReceivedState(void){ }
+ void failsafeOnValidDataReceived(void) { }
+ void failsafeOnValidDataFailed(void) { }
+
+ void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ }
+
+ bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(initialRxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool crsfRxInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback);
+
+ bool jetiExBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataFnPtr *callback)
+ {
+ UNUSED(rxConfig);
+ UNUSED(rxRuntimeConfig);
+ UNUSED(callback);
+ return true;
+ }
+
+ float pt1FilterGain(float f_cut, float dT)
+ {
+ UNUSED(f_cut);
+ UNUSED(dT);
+ return 0.0;
+ }
+
+ void pt1FilterInit(pt1Filter_t *filter, float k)
+ {
+ UNUSED(filter);
+ UNUSED(k);
+ }
+
+ float pt1FilterApply(pt1Filter_t *filter, float input)
+ {
+ UNUSED(filter);
+ UNUSED(input);
+ return 0.0;
+ }
+
+}
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index b9b243e7b0..fc22bd57ec 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -81,6 +81,7 @@ extern "C" {
float motorOutputHigh = 2047;
float motorOutputLow = 1000;
+ linkQualitySource_e linkQualitySource;
acc_t acc;
float accAverage[XYZ_AXIS_COUNT];
@@ -1099,7 +1100,7 @@ extern "C" {
uint8_t getRssiPercent(void) { return scaleRange(rssi, 0, RSSI_MAX_VALUE, 0, 100); }
- uint8_t rxGetLinkQuality(void) { return LINK_QUALITY_MAX_VALUE; }
+ uint16_t rxGetLinkQuality(void) { return LINK_QUALITY_MAX_VALUE; }
uint16_t getCoreTemperatureCelsius(void) { return simulationCoreTemperature; }