Add ELRS OSD and buzzer support

src/core/elrs.c

@@ -20,6 +20,7 @@
 #include "core/battery.h"
 #include "core/common.hh"
 #include "core/ht.h"
+#include "core/msp_displayport.h"
 #include "core/osd.h"
 #include "driver/dm5680.h"
 #include "driver/gpio.h"
@@ -42,7 +43,11 @@ static int record_state;
 static uint32_t record_time = 0;
 static bool headtracking_enabled = false;
 
+uint16_t elrs_osd[HD_VMAX][HD_HMAX];
+static uint16_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
@@ -284,11 +289,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;
         case MSP_SET_HT_ENABLE:
             if (packet.payload_size > 0) {
@@ -364,4 +368,43 @@ void msp_ht_update(uint16_t pan, uint16_t tilt, uint16_t roll) {
 
 bool elrs_headtracking_enabled() {
     return headtracking_enabled;
+}
+
+void elrs_clear_osd() {
+    for (int i = 0; i < HD_VMAX; i++) {
+        for (int j = 0; j < HD_HMAX; j++) {
+            elrs_osd_overlay[i][j] = 0x20;
+        }
+    }
+}
+
+static void handleOSD(uint8_t payload[], uint8_t size) {
+    switch (payload[0]) {
+    case 0x00: // hearbeat
+        break;
+    case 0x01: // release port
+        elrs_clear_osd();
+        memcpy(elrs_osd, elrs_osd_overlay, sizeof(elrs_osd));
+        osd_signal_update();
+        break;
+    case 0x02: // clear screen
+        elrs_clear_osd();
+        break;
+    case 0x03: // write string
+    {
+        uint8_t row = payload[1];
+        uint8_t col = payload[2];
+        uint8_t attr = payload[3];
+        // HDZero only allows 2 pages of characters (i.e. 512 characters)
+        int page = (attr & 1) * 256;
+        uint8_t len = size - 4 < HD_HMAX - col ? size - 4 : HD_HMAX - col;
+        for (int j = 0; j < len; j++) {
+            elrs_osd_overlay[row][col+j] = payload[j+4] + page;
+        }
+    } break;
+    case 0x04: // draw screen
+        memcpy(elrs_osd, elrs_osd_overlay, sizeof(elrs_osd));
+        osd_signal_update();
+        break;
+    }
 }

src/core/elrs.h

@@ -4,6 +4,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "core/msp_displayport.h"
+
 typedef enum {
     MSP_IDLE,
     MSP_HEADER_START,
@@ -59,8 +61,11 @@ typedef struct {
     bool read_error;
 } mspPacket_t;
 
+extern uint16_t elrs_osd[HD_VMAX][HD_HMAX];
+
 void elrs_init();
 bool elrs_headtracking_enabled();
+void elrs_clear_osd();
 
 void msp_send_packet(uint16_t function, mspPacketType_e type, uint16_t payload_size, uint8_t *payload);
 bool msp_read_resposne(uint16_t function, uint16_t *payload_size, uint8_t *payload);

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"
@@ -444,6 +445,7 @@ void osd_hdzero_update(void) {
 
 int osd_clear(void) {
     clear_screen();
+    elrs_clear_osd();
     osd_signal_update();
     return 0;
 }
@@ -678,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);
                 }
             }