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; }