Merge pull request #72 from ExpressLRS/skyzone-custom-osd

Add support for custom OSD on skyzone MSP
ExpressLRS · Feb 12, 2023 · df8dbdb · df8dbdb
2 parents 5eeab17 + e899c45
commit df8dbdb
Show file tree

Hide file tree

Showing 9 changed files with 73 additions and 8 deletions.
diff --git a/lib/MSP/msp.h b/lib/MSP/msp.h
@@ -4,9 +4,9 @@
 
 // TODO: MSP_PORT_INBUF_SIZE should be changed to
 // dynamically allocate array length based on the payload size
-// Hardcoding payload size to 8 bytes for now, since MSP is
-// limited to a 4 byte payload on the BF side
-#define MSP_PORT_INBUF_SIZE 8
+// Hardcoding payload size to 64 bytes for now, to allow enough space
+// for custom OSD text.
+#define MSP_PORT_INBUF_SIZE 64
 
 #define CHECK_PACKET_PARSING() \
   if (packet->readError) {\

diff --git a/lib/MSP/msptypes.h b/lib/MSP/msptypes.h
@@ -16,6 +16,7 @@
 #define MSP_ELRS_REQU_VTX_PKT                   0x0B
 #define MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE      0x0C
 #define MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE     0x0D
+#define MSP_ELRS_SET_OSD                        0x00B6
 
 // CRSF encapsulated msp defines
 #define ENCAPSULATED_MSP_PAYLOAD_SIZE           4

diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp
@@ -1,6 +1,13 @@
 #include <Arduino.h>
-#include <espnow.h>
-#include <ESP8266WiFi.h>
+
+#if defined(PLATFORM_ESP8266)
+  #include <espnow.h>
+  #include <ESP8266WiFi.h>
+#elif defined(PLATFORM_ESP32)
+  #include <esp_now.h>
+  #include <esp_wifi.h>
+  #include <WiFi.h>
+#endif
 
 #include "msp.h"
 #include "msptypes.h"
@@ -52,6 +59,13 @@ void sendMSPViaEspnow(mspPacket_t *packet);
 
 /////////////////////////////////////
 
+#if defined(PLATFORM_ESP32)
+// This seems to need to be global, as per this page,
+// otherwise we get errors about invalid peer:
+// https://rntlab.com/question/espnow-peer-interface-is-invalid/
+esp_now_peer_info_t peerInfo;
+#endif
+
 void RebootIntoWifi()
 {
   DBGLN("Rebooting into wifi update mode...");
@@ -74,7 +88,11 @@ void ProcessMSPPacketFromPeer(mspPacket_t *packet)
 }
 
 // espnow on-receive callback
+#if defined(PLATFORM_ESP8266)
 void OnDataRecv(uint8_t * mac_addr, uint8_t *data, uint8_t data_len)
+#elif defined(PLATFORM_ESP32)
+void OnDataRecv(const uint8_t * mac_addr, const uint8_t *data, int data_len)
+#endif
 {
   DBGLN("ESP NOW DATA:");
   for(int i = 0; i < data_len; i++)
@@ -198,7 +216,11 @@ void SetSoftMACAddress()
   WiFi.disconnect();
 
   // Soft-set the MAC address to the passphrase UID for binding
-  wifi_set_macaddr(STATION_IF, broadcastAddress);
+  #if defined(PLATFORM_ESP8266)
+    wifi_set_macaddr(STATION_IF, broadcastAddress);
+  #elif defined(PLATFORM_ESP32)
+    esp_wifi_set_mac(WIFI_IF_STA, broadcastAddress);
+  #endif
 }
 
 #if defined(PLATFORM_ESP8266)
@@ -257,8 +279,20 @@ void setup()
       rebootTime = millis();
     }
 
-    esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
-    esp_now_add_peer(broadcastAddress, ESP_NOW_ROLE_COMBO, 1, NULL, 0);
+    #if defined(PLATFORM_ESP8266)
+      esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
+      esp_now_add_peer(broadcastAddress, ESP_NOW_ROLE_COMBO, 1, NULL, 0);
+    #elif defined(PLATFORM_ESP32)
+      memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+      peerInfo.channel = 0;
+      peerInfo.encrypt = false;
+      if (esp_now_add_peer(&peerInfo) != ESP_OK)
+      {
+        DBGLN("ESP-NOW failed to add peer");
+        return;
+      }
+    #endif
+
     esp_now_register_recv_cb(OnDataRecv);
   }
 

diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp
@@ -218,6 +218,9 @@ void ProcessMSPPacket(mspPacket_t *packet)
       vrxModule.SetRecordingState(state, delay);
     }
     break;
+  case MSP_ELRS_SET_OSD:
+    vrxModule.SetOSD(packet);
+    break;
   default:
     DBGLN("Unknown command from ESPNOW");
     break;

diff --git a/src/module_base.cpp b/src/module_base.cpp
@@ -16,6 +16,11 @@ ModuleBase::SetRecordingState(uint8_t recordingState, uint16_t delay)
 {
 }
 
+void
+ModuleBase::SetOSD(mspPacket_t *packet)
+{
+}
+
 void
 ModuleBase::Loop(uint32_t now)
 {

diff --git a/src/module_base.h b/src/module_base.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Arduino.h>
+#include "msp.h"
 
 #define VRX_BOOT_DELAY  0
 
@@ -10,5 +11,6 @@ class ModuleBase
     void Init();
     void SendIndexCmd(uint8_t index);
     void SetRecordingState(uint8_t recordingState, uint16_t delay);
+    void SetOSD(mspPacket_t *packet);
     void Loop(uint32_t now);
 };
diff --git a/src/skyzone_msp.cpp b/src/skyzone_msp.cpp
@@ -114,6 +114,13 @@ SkyzoneMSP::SendRecordingState()
     msp.sendPacket(&packet, m_port);
 }
 
+void
+SkyzoneMSP::SetOSD(mspPacket_t *packet)
+{
+    MSP msp;
+    msp.sendPacket(packet, m_port);
+}
+
 void
 SkyzoneMSP::Loop(uint32_t now)
 {

diff --git a/src/skyzone_msp.h b/src/skyzone_msp.h
@@ -26,6 +26,7 @@ class SkyzoneMSP : public ModuleBase
     void SetChannelIndex(uint8_t index);
     uint8_t GetRecordingState();
     void SetRecordingState(uint8_t recordingState, uint16_t delay);
+    void SetOSD(mspPacket_t *packet);
     void Loop(uint32_t now);
 
 private:

diff --git a/targets/debug.ini b/targets/debug.ini
@@ -35,3 +35,15 @@ build_flags =
 
 [env:DEBUG_TX_Backpack_via_WIFI]
 extends = env:DEBUG_TX_Backpack_via_UART
+
+[env:DEBUG_ESP32_TX_Backpack_via_UART]
+extends = env_common_esp32, tx_backpack_common
+build_flags =
+	${env_common_esp32.build_flags}
+	${tx_backpack_common.build_flags}
+	-D PIN_BUTTON=0
+	-D PIN_LED=4
+lib_ignore = STM32UPDATE
+
+[env:DEBUG_ESP32_TX_Backpack_via_WIFI]
+extends = env:DEBUG_ESP32_TX_Backpack_via_UART