diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 5e97c5eeb..0964acca9 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -315,10 +315,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
#ifdef OSD
case OME_VISIBLE:
if (IS_PRINTVALUE(p) && p->data) {
- uint32_t address = (uint32_t)p->data;
- uint16_t *val;
-
- val = (uint16_t *)address;
+ uint16_t *val = (uint16_t *)p->data;
if (VISIBLE(*val)) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
@@ -757,10 +754,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
#ifdef OSD
case OME_VISIBLE:
if (p->data) {
- uint32_t address = (uint32_t)p->data;
- uint16_t *val;
-
- val = (uint16_t *)address;
+ uint16_t *val = (uint16_t *)p->data;
if (key == KEY_RIGHT)
*val |= VISIBLE_FLAG;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index dcbd7257f..aa2903859 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -36,6 +36,7 @@
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
+#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
@@ -88,13 +89,6 @@
#define VIDEO_BUFFER_CHARS_PAL 480
-// Character coordinate
-
-#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
-#define OSD_POS(x,y) ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
-#define OSD_X(x) (x & 0x001F)
-#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & 0x001F)
-
// Blink control
static bool blinkState = true;
@@ -168,9 +162,9 @@ static char osdGetMetersToSelectedUnitSymbol()
{
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
- return 0xF;
+ return SYM_FT;
default:
- return 0xC;
+ return SYM_M;
}
}
@@ -761,7 +755,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
static void osdDrawLogo(int x, int y)
{
// display logo and help
- char fontOffset = 160;
+ unsigned char fontOffset = 160;
for (int row = 0; row < 4; row++) {
for (int column = 0; column < 24; column++) {
if (fontOffset != 255) // FIXME magic number
@@ -810,7 +804,6 @@ void osdUpdateAlarms(void)
// uint16_t *itemPos = osdConfig()->item_pos;
int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
- statRssi = rssi * 100 / 1024;
if (statRssi < osdConfig()->rssi_alarm)
SET_BLINK(OSD_RSSI_VALUE);
@@ -1039,7 +1032,7 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, 12, 7, "ARMED");
}
-static void osdRefresh(timeUs_t currentTimeUs)
+STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static uint8_t lastSec = 0;
uint8_t sec;
@@ -1058,6 +1051,8 @@ static void osdRefresh(timeUs_t currentTimeUs)
armState = ARMING_FLAG(ARMED);
}
+ statRssi = rssi * 100 / 1024;
+
osdUpdateStats();
sec = currentTimeUs / 1000000;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 1b500786e..a5d15e18f 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -26,6 +26,13 @@
#define OSD_POS_MAX 0x3FF
#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values
+// Character coordinate
+
+#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
+#define OSD_POS(x,y) ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
+#define OSD_X(x) (x & 0x001F)
+#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & 0x001F)
+
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
diff --git a/src/main/target/KROOZX/config.c b/src/main/target/KROOZX/config.c
index 05a009b06..e18f3672e 100755
--- a/src/main/target/KROOZX/config.c
+++ b/src/main/target/KROOZX/config.c
@@ -30,8 +30,6 @@
#define VBAT_SCALE 113
-#define OSD_POS(x,y) (x | (y << 5))
-
#ifdef TARGET_CONFIG
void targetConfiguration(void)
{
diff --git a/src/test/Makefile b/src/test/Makefile
index 0407c27bd..4776ac9df 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -107,6 +107,17 @@ maths_unittest_SRC := \
$(USER_DIR)/common/maths.c
+osd_unittest_SRC := \
+ $(USER_DIR)/io/osd.c \
+ $(USER_DIR)/common/typeconversion.c \
+ $(USER_DIR)/drivers/display.c \
+ $(USER_DIR)/common/maths.c \
+ $(USER_DIR)/common/printf.c
+
+osd_unittest_DEFINES := \
+ OSD
+
+
parameter_groups_unittest_SRC := \
$(USER_DIR)/config/parameter_group.c
@@ -359,7 +370,7 @@ $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c
@echo "compiling $$<" "$(STDOUT)"
$(V1) mkdir -p $$(dir $$@)
$(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \
- $(foreach def,$1_DEFINES,-D $(def)) \
+ $(foreach def,$($1_DEFINES),-D $(def)) \
-c $$< -o $$@
@@ -367,7 +378,7 @@ $(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc
@echo "compiling $$<" "$(STDOUT)"
$(V1) mkdir -p $$(dir $$@)
$(V1) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) \
- $(foreach def,$1_DEFINES,-D $(def)) \
+ $(foreach def,$($1_DEFINES),-D $(def)) \
-c $$< -o $$@
diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc
index c422e2473..a61f2a4df 100644
--- a/src/test/unit/cms_unittest.cc
+++ b/src/test/unit/cms_unittest.cc
@@ -28,7 +28,6 @@
extern "C" {
#include "platform.h"
#include "target.h"
- #include "drivers/display.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
void cmsMenuOpen(void);
@@ -38,102 +37,9 @@ extern "C" {
}
#include "unittest_macros.h"
+#include "unittest_displayport.h"
#include "gtest/gtest.h"
-static displayPort_t testDisplayPort;
-static int displayPortTestGrab(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestRelease(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestClearScreen(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestDrawScreen(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestScreenSize(const displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
-{
- UNUSED(displayPort);
- UNUSED(x);
- UNUSED(y);
- UNUSED(s);
- return 0;
-}
-
-static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
-{
- UNUSED(displayPort);
- UNUSED(x);
- UNUSED(y);
- UNUSED(c);
- return 0;
-}
-
-static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static int displayPortTestHeartbeat(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static void displayPortTestResync(displayPort_t *displayPort)
-{
- UNUSED(displayPort);
-}
-
-static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return 0;
-}
-
-static const displayPortVTable_t testDisplayPortVTable = {
- .grab = displayPortTestGrab,
- .release = displayPortTestRelease,
- .clearScreen = displayPortTestClearScreen,
- .drawScreen = displayPortTestDrawScreen,
- .screenSize = displayPortTestScreenSize,
- .write = displayPortTestWrite,
- .writeChar = displayPortTestWriteChar,
- .isTransferInProgress = displayPortTestIsTransferInProgress,
- .heartbeat = displayPortTestHeartbeat,
- .resync = displayPortTestResync,
- .txBytesFree = displayPortTestTxBytesFree
-};
-
-displayPort_t *displayPortTestInit(void)
-{
- displayInit(&testDisplayPort, &testDisplayPortVTable);
- testDisplayPort.rows = 10;
- testDisplayPort.cols = 40;
- return &testDisplayPort;
-}
-
TEST(CMSUnittest, TestCmsDisplayPortRegister)
{
cmsInit();
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
new file mode 100644
index 000000000..dcccdfc61
--- /dev/null
+++ b/src/test/unit/osd_unittest.cc
@@ -0,0 +1,568 @@
+/*
+ * 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 "config/parameter_group_ids.h"
+ #include "drivers/max7456_symbols.h"
+ #include "fc/config.h"
+ #include "fc/rc_controls.h"
+ #include "fc/rc_modes.h"
+ #include "fc/runtime_config.h"
+ #include "flight/pid.h"
+ #include "flight/imu.h"
+ #include "io/osd.h"
+ #include "sensors/battery.h"
+ #include "rx/rx.h"
+
+ void osdRefresh(timeUs_t currentTimeUs);
+
+ uint8_t stateFlags;
+ uint8_t armingFlags;
+ uint16_t flightModeFlags;
+ uint16_t rssi;
+ attitudeEulerAngles_t attitude;
+ pidProfile_t *currentPidProfile;
+ int16_t debug[DEBUG16_VALUE_COUNT];
+ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+ uint8_t GPS_numSat;
+ uint16_t GPS_speed;
+ uint16_t GPS_distanceToHome;
+ uint16_t GPS_directionToHome;
+ int32_t GPS_coord[2];
+
+ 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);
+
+ timeUs_t simulationTime = 0;
+ batteryState_e simulationBatteryState;
+ uint8_t simulationBatteryCellCount;
+ uint16_t simulationBatteryVoltage;
+ int32_t simulationAltitude;
+ int32_t simulationVerticalSpeed;
+}
+
+/* #define DEBUG_OSD */
+
+#include "unittest_macros.h"
+#include "unittest_displayport.h"
+#include "gtest/gtest.h"
+
+void setDefualtSimulationState()
+{
+ rssi = 1024;
+
+ simulationBatteryState = BATTERY_OK;
+ simulationBatteryCellCount = 4;
+ simulationBatteryVoltage = 168;
+ simulationAltitude = 0;
+ simulationVerticalSpeed = 0;
+}
+
+/*
+ * 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 += 5e5;
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+
+ // then
+ // arming alert disappears
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+ if (testEmpty) {
+ for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+ EXPECT_EQ(' ', testDisplayPortBuffer[i]);
+ }
+ }
+}
+
+/*
+ * 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
+ displayPortTestBufferSubstring(2, 2, " --- STATS ---");
+}
+
+
+/*
+ * Tests initialisation of the OSD and the power on splash screen.
+ */
+TEST(OsdTest, TestInit)
+{
+ // given
+ // display port is initialised
+ displayPortTestInit();
+
+ // and
+ // default state values are set
+ setDefualtSimulationState();
+
+ // and
+ // this battery configuration (used for battery voltage elements)
+ batteryConfigMutable()->vbatmincellvoltage = 33;
+ batteryConfigMutable()->vbatmaxcellvoltage = 43;
+
+ // 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 visibility of the ARMED notification after arming.
+ */
+TEST(OsdTest, TestArm)
+{
+ doTestArm();
+}
+
+/*
+ * Tests display and timeout of the post flight statistics screen after disarming.
+ */
+TEST(OsdTest, TestDisarm)
+{
+ doTestDisarm();
+
+ // given
+ // post flight stats times out (60 seconds)
+ simulationTime += 60e6;
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+
+ // then
+ // post flight stats screen disappears
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+ displayPortTestBufferIsEmpty();
+}
+
+/*
+ * Tests disarming and immediately rearming clears post flight stats and shows ARMED notification.
+ */
+TEST(OsdTest, TestDisarmWithImmediateRearm)
+{
+ doTestArm();
+ doTestDisarm();
+ doTestArm();
+}
+
+/*
+ * Tests dismissing the statistics screen with pitch stick after disarming.
+ */
+TEST(OsdTest, TestDisarmWithDismissStats)
+{
+ // Craft is alread armed after previous test
+
+ doTestDisarm();
+
+ // given
+ // sticks have been moved
+ rcData[PITCH] = 1800;
+
+ // when
+ // sufficient OSD updates have been called
+ osdRefresh(simulationTime);
+ osdRefresh(simulationTime);
+
+ // then
+ // post flight stats screen disappears
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+ displayPortTestBufferIsEmpty();
+
+ rcData[PITCH] = 1500;
+}
+
+/*
+ * Tests the calculation of statistics with imperial unit output.
+ */
+TEST(OsdTest, TestStatsImperial)
+{
+ // given
+ // this set of enabled post flight statistics
+ osdConfigMutable()->enabled_stats[OSD_STAT_MAX_SPEED] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_MIN_RSSI] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_MAX_CURRENT] = false;
+ osdConfigMutable()->enabled_stats[OSD_STAT_USED_MAH] = false;
+ osdConfigMutable()->enabled_stats[OSD_STAT_MAX_ALTITUDE] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX] = false;
+ osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_FLYTIME] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_ARMEDTIME] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE] = true;
+ osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false;
+
+ // and
+ // using imperial unit system
+ osdConfigMutable()->units = OSD_UNIT_IMPERIAL;
+
+ // and
+ // a GPS fix is present
+ stateFlags |= GPS_FIX | GPS_FIX_HOME;
+
+ // when
+ // the craft is armed
+ doTestArm();
+
+ // and
+ // these conditions occur during flight
+ rssi = 1024;
+ GPS_speed = 500;
+ GPS_distanceToHome = 20;
+ simulationBatteryVoltage = 158;
+ simulationAltitude = 100;
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+ rssi = 512;
+ GPS_speed = 800;
+ GPS_distanceToHome = 50;
+ simulationBatteryVoltage = 147;
+ simulationAltitude = 150;
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+ rssi = 256;
+ GPS_speed = 200;
+ GPS_distanceToHome = 100;
+ simulationBatteryVoltage = 152;
+ simulationAltitude = 200;
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+ // and
+ // the craft is disarmed
+ doTestDisarm();
+
+ // then
+ // statistics screen should display the following
+ displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:04");
+ displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:07");
+ displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28");
+ displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 328%c", SYM_FT);
+ displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT);
+ displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT);
+ displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%");
+ displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 6.5%c", SYM_FT);
+}
+
+/*
+ * Tests the calculation of statistics with metric unit output.
+ * (essentially an abridged version of the previous test
+ */
+TEST(OsdTest, TestStatsMetric)
+{
+ // given
+ // using metric unit system
+ osdConfigMutable()->units = OSD_UNIT_METRIC;
+
+ // and
+ // default state values are set
+ setDefualtSimulationState();
+
+ // when
+ // the craft is armed
+ doTestArm();
+
+ // and
+ // these conditions occur during flight (simplified to less assignments than previous test)
+ rssi = 256;
+ GPS_speed = 800;
+ GPS_distanceToHome = 100;
+ simulationBatteryVoltage = 147;
+ simulationAltitude = 200;
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+ osdRefresh(simulationTime);
+
+ simulationBatteryVoltage = 152;
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+ // and
+ // the craft is disarmed
+ doTestDisarm();
+
+ // then
+ // statistics screen should display the following
+ displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:02");
+ displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:09");
+ displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28");
+ displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 100%c", SYM_M);
+ displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT);
+ displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT);
+ displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%");
+ displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 2.0%c", SYM_M);
+}
+
+/*
+ * Tests activation of alarms and element flashing.
+ */
+TEST(OsdTest, TestAlarms)
+{
+ // given
+ // default state is set
+ setDefualtSimulationState();
+
+ // and
+ // the following OSD elements are visible
+ osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
+ osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG;
+ osdConfigMutable()->item_pos[OSD_FLYTIME] = OSD_POS(1, 1) | VISIBLE_FLAG;
+ osdConfigMutable()->item_pos[OSD_ALTITUDE] = OSD_POS(23, 7) | VISIBLE_FLAG;
+
+ // and
+ // this set of alarm values
+ osdConfigMutable()->rssi_alarm = 20;
+ osdConfigMutable()->cap_alarm = 2200;
+ osdConfigMutable()->time_alarm = 1; // in minutes
+ osdConfigMutable()->alt_alarm = 100; // meters
+
+ // and
+ // using the metric unit system
+ osdConfigMutable()->units = OSD_UNIT_METRIC;
+
+ // when
+ // the craft is armed
+ doTestArm(false);
+
+ // then
+ // no elements should flash as all values are out of alarm range
+ for (int i = 0; i < 6; i++) {
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+ printf("%d\n", i);
+#endif
+ displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
+ displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT);
+ displayPortTestBufferSubstring(1, 1, "%c00:", SYM_FLY_M); // only test the minute part of the timer
+ displayPortTestBufferSubstring(23, 7, " 0.0%c", SYM_M);
+ }
+
+ // when
+ // all values are out of range
+ rssi = 128;
+ simulationBatteryState = BATTERY_CRITICAL;
+ simulationBatteryVoltage = 135;
+ simulationAltitude = 12000;
+ // Fly timer is incremented on periodic calls to osdRefresh, can't simply just increment the simulated system clock
+ for (int i = 0; i < 60; i++) {
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+ }
+
+ // then
+ // elements showing values in alarm range should flash
+ for (int i = 0; i < 6; i++) {
+ simulationTime += 1e6;
+ osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+ printf("%d\n", i);
+ displayPortTestPrint();
+#endif
+ if (i % 2 == 1) {
+ displayPortTestBufferSubstring(8, 1, "%c12", SYM_RSSI);
+ displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
+ displayPortTestBufferSubstring(1, 1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
+ displayPortTestBufferSubstring(23, 7, " 120.0%c", SYM_M);
+ } else {
+ displayPortTestBufferIsEmpty();
+ }
+ }
+}
+
+/*
+ * Tests the RSSI OSD element.
+ */
+TEST(OsdTest, TestElementRssi)
+{
+ // given
+ osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
+ osdConfigMutable()->rssi_alarm = 0;
+
+ // when
+ rssi = 1024;
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // then
+ displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
+
+ // when
+ rssi = 0;
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // then
+ displayPortTestBufferSubstring(8, 1, "%c0", SYM_RSSI);
+
+ // when
+ rssi = 512;
+ displayClearScreen(&testDisplayPort);
+ osdRefresh(simulationTime);
+
+ // then
+ displayPortTestBufferSubstring(8, 1, "%c50", SYM_RSSI);
+}
+
+
+// STUBS
+extern "C" {
+ bool sensors(uint32_t mask) {
+ UNUSED(mask);
+ return true;
+ }
+
+ bool IS_RC_MODE_ACTIVE(boxId_e boxId) {
+ UNUSED(boxId);
+ return false;
+ }
+
+ uint32_t micros() {
+ return simulationTime;
+ }
+
+ bool isBeeperOn() {
+ return false;
+ }
+
+ bool isAirmodeActive() {
+ return false;
+ }
+
+ uint8_t getCurrentPidProfileIndex() {
+ return 0;
+ }
+
+ uint8_t getCurrentControlRateProfileIndex() {
+ return 0;
+ }
+
+ batteryState_e getBatteryState() {
+ return simulationBatteryState;
+ }
+
+ uint8_t getBatteryCellCount() {
+ return simulationBatteryCellCount;
+ }
+
+ uint16_t getBatteryVoltage() {
+ return simulationBatteryVoltage;
+ }
+
+ int32_t getAmperage() {
+ return 0;
+ }
+
+ int32_t getMAhDrawn() {
+ return 0;
+ }
+
+ int32_t getEstimatedAltitude() {
+ return simulationAltitude;
+ }
+
+ int32_t getEstimatedVario() {
+ return simulationVerticalSpeed;
+ }
+
+ unsigned int blackboxGetLogNumber() {
+ return 0;
+ }
+
+ bool isSerialTransmitBufferEmpty(const serialPort_t *instance) {
+ UNUSED(instance);
+ return false;
+ }
+
+ void serialWrite(serialPort_t *instance, uint8_t ch) {
+ UNUSED(instance);
+ UNUSED(ch);
+ }
+
+ bool cmsDisplayPortRegister(displayPort_t *pDisplay) {
+ UNUSED(pDisplay);
+ return false;
+ }
+}
diff --git a/src/test/unit/unittest_displayport.h b/src/test/unit/unittest_displayport.h
new file mode 100644
index 000000000..a2cd7d635
--- /dev/null
+++ b/src/test/unit/unittest_displayport.h
@@ -0,0 +1,163 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+
+extern "C" {
+ #include "drivers/display.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+#define UNITTEST_DISPLAYPORT_ROWS 16
+#define UNITTEST_DISPLAYPORT_COLS 30
+#define UNITTEST_DISPLAYPORT_BUFFER_LEN (UNITTEST_DISPLAYPORT_ROWS * UNITTEST_DISPLAYPORT_COLS)
+
+char testDisplayPortBuffer[UNITTEST_DISPLAYPORT_BUFFER_LEN];
+
+static displayPort_t testDisplayPort;
+
+static int displayPortTestGrab(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int displayPortTestRelease(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int displayPortTestClearScreen(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ memset(testDisplayPortBuffer, ' ', UNITTEST_DISPLAYPORT_BUFFER_LEN);
+ return 0;
+}
+
+static int displayPortTestDrawScreen(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int displayPortTestScreenSize(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+{
+ UNUSED(displayPort);
+ for (unsigned int i = 0; i < strlen(s); i++) {
+ testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x + i] = s[i];
+ }
+ return 0;
+}
+
+static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
+{
+ UNUSED(displayPort);
+ testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x] = c;
+ return 0;
+}
+
+static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int displayPortTestHeartbeat(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static void displayPortTestResync(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+}
+
+static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static const displayPortVTable_t testDisplayPortVTable = {
+ .grab = displayPortTestGrab,
+ .release = displayPortTestRelease,
+ .clearScreen = displayPortTestClearScreen,
+ .drawScreen = displayPortTestDrawScreen,
+ .screenSize = displayPortTestScreenSize,
+ .write = displayPortTestWrite,
+ .writeChar = displayPortTestWriteChar,
+ .isTransferInProgress = displayPortTestIsTransferInProgress,
+ .heartbeat = displayPortTestHeartbeat,
+ .resync = displayPortTestResync,
+ .txBytesFree = displayPortTestTxBytesFree
+};
+
+displayPort_t *displayPortTestInit(void)
+{
+ displayInit(&testDisplayPort, &testDisplayPortVTable);
+ testDisplayPort.rows = UNITTEST_DISPLAYPORT_ROWS;
+ testDisplayPort.cols = UNITTEST_DISPLAYPORT_COLS;
+ return &testDisplayPort;
+}
+
+void displayPortTestPrint(void)
+{
+ for (int i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+ if (i > 0 && i % UNITTEST_DISPLAYPORT_COLS == 0) {
+ printf("\n");
+ }
+ printf("%c", testDisplayPortBuffer[i]);
+ }
+ printf("\n\n");
+}
+
+void displayPortTestBufferIsEmpty()
+{
+ for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+ EXPECT_EQ(' ', testDisplayPortBuffer[i]);
+ }
+}
+
+void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...)
+{
+ char expected[32];
+
+ va_list args;
+ va_start(args, expectedFormat);
+ vsprintf(expected, expectedFormat, args);
+ va_end(args);
+
+#ifdef DEBUG_OSD
+ displayPortTestPrint();
+#endif
+
+ for (size_t i = 0; i < strlen(expected); i++) {
+ EXPECT_EQ(expected[i], testDisplayPortBuffer[(y * testDisplayPort.cols) + x + i]);
+ }
+}