From 843a25903a8625dc7e5d82eb65e8c09c9ddc574c Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Tue, 12 Jun 2018 20:04:06 -0400
Subject: [PATCH] Display OSD message and countdown if arming is delayed due to
 beacon Provides a clear indication that arming is delayed for cases where
 DSHOT beacon is active.

Clears the OSD and displays "DISABLING BEACON" and "ARMING IN X.Y" with an active countdown in tenths of a second while arming is delayed due to DSHOT beacon. Once delay period is over the normal "ARMING" message appears.

If the DSHOT beacon is not active then this delay screen is not displayed.
---
 src/main/io/osd.c             | 31 ++++++++++++++++++++++++++++++-
 src/test/unit/osd_unittest.cc |  1 +
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index c0829e271..5339f031e 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -202,6 +202,13 @@ static const uint8_t osdElementDisplayOrder[] = {
 
 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);
 
+static void osdDisplayCenteredMessage(int row, const char *message)
+{
+    const int messageLen = strlen(message);
+    const int col = (messageLen >= osdDisplayPort->cols) ? 0 : (osdDisplayPort->cols - strlen(message)) / 2;
+    displayWrite(osdDisplayPort, col, row, message);
+}
+
 /**
  * Gets the correct altitude symbol for the current unit system
  */
@@ -1390,9 +1397,24 @@ static void osdShowStats(uint16_t endBatteryVoltage)
 static void osdShowArmed(void)
 {
     displayClearScreen(osdDisplayPort);
-    displayWrite(osdDisplayPort, 12, 7, "ARMED");
+    osdDisplayCenteredMessage(7, "ARMED");
 }
 
+#ifdef USE_DSHOT
+static void osdShowTryingToArm(timeUs_t currentTimeUs)
+{
+    char buff[14];
+    int delayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5;
+    if (delayTime < 0) {
+        delayTime = 0;
+    }
+    displayClearScreen(osdDisplayPort);
+    osdDisplayCenteredMessage(5, "DISABLING BEACON");
+    tfp_sprintf(buff, "ARMING IN %d.%d", delayTime / 10, delayTime % 10);
+    osdDisplayCenteredMessage(7, buff);
+}
+#endif
+
 STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
 {
     static timeUs_t lastTimeUs = 0;
@@ -1401,6 +1423,13 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
     static timeUs_t osdStatsRefreshTimeUs;
     static uint16_t endBatteryVoltage;
 
+#ifdef USE_DSHOT
+    if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
+        osdShowTryingToArm(currentTimeUs);
+        return;
+    }
+#endif
+
     // detect arm/disarm
     if (armState != ARMING_FLAG(ARMED)) {
         if (ARMING_FLAG(ARMED)) {
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index 353fddcc9..28d076ac6 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -45,6 +45,7 @@ extern "C" {
     #include "flight/pid.h"
     #include "flight/imu.h"
 
+    #include "io/beeper.h"
     #include "io/gps.h"
     #include "io/osd.h"
 
-- 
2.11.4.GIT