diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index 6eca978a..cda93bc9 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/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 diff --git a/lib/MSP/msptypes.h b/lib/MSP/msptypes.h index 2c05fbb5..ad9c2364 100644 --- a/lib/MSP/msptypes.h +++ b/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 diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index e9921275..c2b7530e 100644 --- a/src/Vrx_main.cpp +++ b/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: diff --git a/src/hdzero.cpp b/src/hdzero.cpp index 12802e83..be0ede45 100644 --- a/src/hdzero.cpp +++ b/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); +} diff --git a/src/hdzero.h b/src/hdzero.h index cf9fee28..c5e8bec5 100644 --- a/src/hdzero.h +++ b/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); }; diff --git a/src/module_base.h b/src/module_base.h index cace5ab6..1963af42 100644 --- a/src/module_base.h +++ b/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); };