Add ELRS OSD and buzzer support

hd-zero · Feb 21, 2023 · 70372d6 · 70372d6
1 parent fef1422
commit 70372d6
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 5 deletions.
diff --git a/src/core/elrs.c b/src/core/elrs.c
@@ -19,6 +19,7 @@
 
 #include "core/battery.h"
 #include "core/common.hh"
+#include "core/msp_displayport.h"
 #include "core/osd.h"
 #include "driver/dm5680.h"
 #include "driver/gpio.h"
@@ -40,7 +41,11 @@ static mspPacket_t response_packet;
 static int record_state;
 static uint32_t record_time = 0;
 
+uint8_t elrs_osd[HD_VMAX][HD_HMAX];
+static uint8_t elrs_osd_overlay[HD_VMAX][HD_HMAX];
+
 void msp_process_packet();
+static void handleOSD(uint8_t *payload, uint8_t size);
 
 static const uint16_t freq_table[] = {
     5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // R1-8
@@ -282,11 +287,10 @@ void msp_process_packet() {
             }
         } break;
         case MSP_SET_BUZZER:
-            // TODO after merge
-            // beep_dur((packet.payload[0] | packet.payload[1]<<8) * 1000);
+            beep_dur((packet.payload[0] | packet.payload[1]<<8) * 1000);
             break;
         case MSP_SET_OSD_ELEM:
-            // TODO
+            handleOSD(packet.payload, packet.payload_size);
             break;
         }
     } else if (packet.type == MSP_PACKET_RESPONSE) {
@@ -344,3 +348,34 @@ void msp_send_packet(uint16_t function, mspPacketType_e type, uint16_t payload_s
     buffer[payload_size + 8] = crc;
     uart_write(fd_esp32, buffer, payload_size + 9);
 }
+
+static void clear_osd() {
+    memset(elrs_osd_overlay, 0x20, sizeof(elrs_osd_overlay));
+}
+
+static void handleOSD(uint8_t payload[], uint8_t size) {
+    switch (payload[0]) {
+    case 0x00: // hearbeat
+        break;
+    case 0x01: // release port
+        clear_osd();
+        memcpy(elrs_osd, elrs_osd_overlay, sizeof(elrs_osd));
+        osd_signal_update();
+        break;
+    case 0x02: // clear screen
+        clear_osd();
+        break;
+    case 0x03: // write string
+    {
+        uint8_t row = payload[1];
+        uint8_t col = payload[2];
+        uint8_t attr = payload[3];
+        uint8_t len = size - 4 < HD_HMAX - col ? size - 4 : HD_HMAX - col;
+        memcpy(&elrs_osd_overlay[row][col], &payload[4], len);
+    } break;
+    case 0x04: // draw screen
+        memcpy(elrs_osd, elrs_osd_overlay, sizeof(elrs_osd));
+        osd_signal_update();
+        break;
+    }
+}
diff --git a/src/core/osd.c b/src/core/osd.c
@@ -18,6 +18,7 @@
 
 #include "core/battery.h"
 #include "core/common.hh"
+#include "core/elrs.h"
 #include "core/msp_displayport.h"
 #include "driver/dm5680.h"
 #include "driver/fans.h"
@@ -41,6 +42,8 @@ static uint16_t osd_buf_shadow[HD_VMAX][HD_HMAX];
 extern lv_style_t style_osd;
 extern pthread_mutex_t lvgl_mutex;
 
+extern uint8_t elrs_osd[HD_VMAX][HD_HMAX];
+
 ///////////////////////////////////////////////////////////////////
 //-1=error;
 // 0=idle,1=recording,2=stopped,3=No SD card,4=recorf file path error,
@@ -677,8 +680,10 @@ void *thread_osd(void *ptr) {
 
         for (int i = 0; i < HD_VMAX; i++) {
             for (int j = 0; j < HD_HMAX; j++) {
-                if (osd_buf[i][j] != osd_buf_shadow[i][j]) {
-                    osd_buf_shadow[i][j] = osd_buf[i][j];
+                uint16_t ch = osd_buf[i][j];
+                if (ch == 0x20) ch = elrs_osd[i][j];
+                if (ch != osd_buf_shadow[i][j]) {
+                    osd_buf_shadow[i][j] = ch;
                     draw_osd_on_screen(i, j);
                 }
             }