HDZero VRx: forward telemetry frames to the host

lib/CrsfProtocol/crsf_protocol.h

@@ -6,6 +6,7 @@
 #define CRSF_FRAMETYPE_GPS 0x02
 #define CRSF_FRAMETYPE_LINK_STATISTICS 0x14
 #define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08
+#define CRSF_FRAMETYPE_FLIGHT_MODE 0x21
 
 #define CRSF_CHANNEL_VALUE_1000 191
 #define CRSF_CHANNEL_VALUE_MID  992

lib/MSP/msptypes.h

@@ -50,6 +50,7 @@
 #define MSP_ELRS_BACKPACK_SET_OSD_ELEMENT       0x030C
 #define MSP_ELRS_BACKPACK_SET_HEAD_TRACKING     0x030D  // enable/disable head-tracking forwarding packets to the TX
 #define MSP_ELRS_BACKPACK_SET_RTC               0x030E
+#define MSP_ELRS_BACKPACK_FWD_TLM               0x0310  // raw CRSF telemetry frame forwarded to the goggle host
 
 // incoming, packets originating from the VRx
 #define MSP_ELRS_BACKPACK_SET_MODE              0x0380  // enable wifi/binding mode

src/Vrx_main.cpp

@@ -270,6 +270,9 @@ void ProcessMSPPacket(mspPacket_t *packet)
     case CRSF_FRAMETYPE_LINK_STATISTICS:
       vrxModule.SendLinkTelemetry(packet->payload);
       break;
+    case CRSF_FRAMETYPE_FLIGHT_MODE:
+      vrxModule.SendFlightModeTelemetry(packet->payload);
+      break;
     }
     break;
   default:

src/hdzero.cpp

@@ -139,3 +139,42 @@ HDZero::SetRTC()
 
     msp.sendPacket(&packet, m_port);
 }
+
+void
+HDZero::ForwardCrsfFrame(uint8_t *rawCrsfPacket)
+{
+    // rawCrsfPacket[1] counts type + payload + crc; +2 for sync and size bytes
+    uint16_t len = (uint16_t)rawCrsfPacket[1] + 2;
+    if (len > MSP_PORT_INBUF_SIZE)
+    {
+        return;
+    }
+    MSP msp;
+    mspPacket_t packet;
+    packet.reset();
+    packet.makeCommand();
+    packet.function = MSP_ELRS_BACKPACK_FWD_TLM;
+    for (uint16_t i = 0; i < len; i++)
+    {
+        packet.addByte(rawCrsfPacket[i]);
+    }
+    msp.sendPacket(&packet, m_port);
+}
+
+void
+HDZero::SendGpsTelemetry(crsf_packet_gps_t *packet)
+{
+    ForwardCrsfFrame((uint8_t *)packet);
+}
+
+void
+HDZero::SendFlightModeTelemetry(uint8_t *rawCrsfPacket)
+{
+    ForwardCrsfFrame(rawCrsfPacket);
+}
+
+void
+HDZero::SendLinkTelemetry(uint8_t *rawCrsfPacket)
+{
+    ForwardCrsfFrame(rawCrsfPacket);
+}

src/hdzero.h

@@ -26,4 +26,10 @@ class HDZero : public MSPModuleBase
     void SendHeadTrackingEnableCmd(bool enable);
     void SetOSD(mspPacket_t *packet);
     void SetRTC();
+    void SendGpsTelemetry(crsf_packet_gps_t *packet);
+    void SendFlightModeTelemetry(uint8_t *rawCrsfPacket);
+    void SendLinkTelemetry(uint8_t *rawCrsfPacket);
+
+private:
+    void ForwardCrsfFrame(uint8_t *rawCrsfPacket);
 };

src/module_base.h

@@ -16,6 +16,7 @@ class ModuleBase
     void SendLinkTelemetry(uint8_t *rawCrsfPacket);
     void SendBatteryTelemetry(uint8_t *rawCrsfPacket);
     void SendGpsTelemetry(crsf_packet_gps_t *packet) {}
+    void SendFlightModeTelemetry(uint8_t *rawCrsfPacket) {}
     void Loop(uint32_t now);
 };