From 396296d45cacdc02e652131dc2f40dea8b7c92a5 Mon Sep 17 00:00:00 2001
From: Englebert <=>
Date: Mon, 11 Oct 2021 15:17:26 +0800
Subject: [PATCH] With Diversity mode

---
 BertFPVDiversity.h   |  1 +
 BertFPVDiversity.ino | 64 +++++++++++++++++++++++++++++++++++++++++++-
 main.cpp             | 18 +++++++++++++
 video.cpp            |  1 +
 4 files changed, 83 insertions(+), 1 deletion(-)

diff --git a/BertFPVDiversity.h b/BertFPVDiversity.h
index 85cd8df..8c4b88d 100644
--- a/BertFPVDiversity.h
+++ b/BertFPVDiversity.h
@@ -17,6 +17,7 @@ extern uint8_t display_screen;
 extern uint32_t current_millis;
 extern uint32_t rssi1, rssi2;
 extern uint8_t vtx_channel;
+extern uint8_t current_receiver;
 
 void IRAM_ATTR onTimer();
 void display_output();
diff --git a/BertFPVDiversity.ino b/BertFPVDiversity.ino
index b8dc947..aec7155 100644
--- a/BertFPVDiversity.ino
+++ b/BertFPVDiversity.ino
@@ -47,6 +47,14 @@ uint8_t vtx_channel = 0;
 
 uint32_t last_display = 0;
 
+/* Timing to switch */
+// uint32_t diversity_next_switch = 0;
+int diversity_stable_count = 0;
+uint8_t current_receiver = 0;
+
+// Pinning the video to core 0
+TaskHandle_t VideoCoreTask;
+
 // Starting up WebServer
 WebServer server(80);
 
@@ -626,6 +634,14 @@ void setup_rx5808() {
     RX5808::begin();
 }
 
+// Video core priority to process....
+void VideoCoreProcessing(void *pvParameters) {
+    // Forever loop ....
+    for(;;) {
+        if(display_output_state) display_output();
+    }
+}
+
 void setup() {
     // Preparing Timer Hardware
     /* 1 Aick take 1/(80MHZ/80) = 1us so we set divider 80 and count up */
@@ -664,6 +680,18 @@ void setup() {
     /* Setup Finish Routine */
     setup_done();
 
+    /* Setting up Video on Core 0 */
+    /**
+    xTaskCreatePinnedToCore(
+        VideoCoreProcessing,
+        "VideoCoreTask",
+        8192,
+        NULL,
+        255,
+        &VideoCoreTask,
+        0);
+      **/
+
     /* Cool down a bit... */
     delay(500);
     osd(1);
@@ -712,7 +740,7 @@ bool touchpad(uint8_t touch_id) {
         else
             return false;
     } else if(touch_id == TOUCH_DOWN) {
-        Serial.println(average_reads);
+        // Serial.println(average_reads);
         if(average_reads < THRESHOLD_DOWN)
             return true;
         else
@@ -730,6 +758,20 @@ bool touchpad(uint8_t touch_id) {
     }
 }
 
+void receiverSelector(uint8_t id) {
+    if(id == 0) {
+        digitalWrite(VIDEO_SELECTOR, LOW);
+        digitalWrite(INDICATOR_V1, HIGH);
+        digitalWrite(INDICATOR_V2, LOW);
+        current_receiver = 0;
+    } else if(id == 1) {
+        digitalWrite(VIDEO_SELECTOR, HIGH);
+        digitalWrite(INDICATOR_V1, LOW);
+        digitalWrite(INDICATOR_V2, HIGH);
+        current_receiver = 1;
+    }
+}
+
 void loop() {
     // Get readings from both RX5808 module
     if(millis() > last_rssi_read) {
@@ -741,6 +783,26 @@ void loop() {
         rssi2 = rssi2 >> 2;     // Faster way to divide 4
         rssi1 = (rssi1 > MAX_RSSI) ? MAX_RSSI : rssi1;
         rssi2 = (rssi2 > MAX_RSSI) ? MAX_RSSI : rssi2;
+
+
+        if(rssi1 > rssi2) {
+            diversity_stable_count--;
+        } else if(rssi2 > rssi1) {
+            diversity_stable_count++;
+        }
+
+        if(diversity_stable_count < -5) {
+            receiverSelector(0);
+
+            // Reset counter
+            diversity_stable_count = 0;
+        } else if(diversity_stable_count > 5) {
+            receiverSelector(1);
+
+            // Reset counter
+            diversity_stable_count = 0;
+        }
+
         last_rssi_read = millis() + 10;
     }
 
diff --git a/main.cpp b/main.cpp
index b290642..ebfa82a 100644
--- a/main.cpp
+++ b/main.cpp
@@ -27,6 +27,24 @@ namespace Main {
         uint8_t rssi2_bar = map(rssi2, 0, MAX_RSSI, 0, 255);
         signalbar(rssi1_bar, rssi2_bar);
 
+        // Display current Receiver
+        uint8_t receiver1_color, receiver2_color;
+        if(current_receiver == 0) {
+            receiver1_color = 0x1C;
+            receiver2_color = 0x29;
+        } else {
+            receiver1_color = 0x29;
+            receiver2_color = 0x1C;
+        }
+
+        Video::video.setCursor(110, 13);
+        Video::video.setTextColor(receiver1_color);
+        Video::video.print("A");
+
+        Video::video.setCursor(130, 13);
+        Video::video.setTextColor(receiver2_color);
+        Video::video.print("B");
+
         // Display cubes...
         /*
          * +---+---+---+---+---+---+---+---+
diff --git a/video.cpp b/video.cpp
index 42ce1be..84e6baa 100644
--- a/video.cpp
+++ b/video.cpp
@@ -3,6 +3,7 @@
 namespace Video {
     // Create an instance of the graphics library
     ESP_8_BIT_GFX video(false /* = NTSC */, 8 /* = RGB332 color */);
+    // ESP_8_BIT_GFX video(true /* = NTSC */, 8 /* = RGB332 color */);
 
     void begin() {
         video.begin();