diff --git a/lib/MSP/msp.cpp b/lib/MSP/msp.cpp index ff1ac10..04b2e8e 100644 --- a/lib/MSP/msp.cpp +++ b/lib/MSP/msp.cpp @@ -97,7 +97,10 @@ MSP::processReceivedByte(uint8_t c) m_packet.flags = header->flags; // reset the offset iterator for re-use in payload below m_offset = 0; - m_inputState = MSP_PAYLOAD_V2_NATIVE; + if (m_packet.payloadSize == 0) + m_inputState = MSP_CHECKSUM_V2_NATIVE; + else + m_inputState = MSP_PAYLOAD_V2_NATIVE; } break; diff --git a/lib/MSP/msptypes.h b/lib/MSP/msptypes.h index aae18c1..8be1997 100644 --- a/lib/MSP/msptypes.h +++ b/lib/MSP/msptypes.h @@ -16,6 +16,9 @@ #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_RX_WIFI_MODE 0x0E +#define MSP_ELRS_SET_RX_LOAN_MODE 0x0F +#define MSP_ELRS_GET_BACKPACK_VERSION 0x10 #define MSP_ELRS_SET_OSD 0x00B6 // CRSF encapsulated msp defines diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 9d7c265..9b8e8a2 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -30,6 +30,8 @@ uint8_t broadcastAddress[6] = {0, 0, 0, 0, 0, 0}; #endif uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; +const uint8_t version[] = {LATEST_VERSION}; + connectionState_e connectionState = starting; unsigned long rebootTime = 0; @@ -116,6 +118,19 @@ void OnDataRecv(const uint8_t * mac_addr, const uint8_t *data, int data_len) blinkLED(); } +void SendVersionResponse() +{ + mspPacket_t out; + out.reset(); + out.makeResponse(); + out.function = MSP_ELRS_GET_BACKPACK_VERSION; + for (size_t i = 0 ; i < sizeof(version) ; i++) + { + out.addByte(version[i]); + } + msp.sendPacket(&out, &Serial); +} + void ProcessMSPPacketFromTX(mspPacket_t *packet) { if (packet->function == MSP_ELRS_BIND) @@ -152,6 +167,10 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) DBGLN("Processing MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE..."); RebootIntoWifi(); break; + case MSP_ELRS_GET_BACKPACK_VERSION: + DBGLN("Processing MSP_ELRS_GET_BACKPACK_VERSION..."); + SendVersionResponse(); + break; default: // transparently forward MSP packets via espnow to any subscribers sendMSPViaEspnow(packet);