feat(aoa): replace custom CRSF AOA frame with MSP over CRSF telemetry

Author: Miki4751

src/main/rx/crsf.h

@@ -49,7 +49,6 @@ enum {
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
-    CRSF_FRAME_AOA_PAYLOAD_SIZE = 4,
     CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
     CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
     CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
@@ -105,7 +104,6 @@ typedef enum {
     CRSF_FRAMETYPE_MSP_RESP = 0x7B,  // reply with 58 byte chunked binary
     CRSF_FRAMETYPE_MSP_WRITE = 0x7C,  // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
     CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
-    CRSF_FRAMETYPE_AOA = 0x80,
 } crsfFrameType_e;
 
 enum {

src/main/telemetry/crsf.c

@@ -436,16 +436,33 @@ uint8_t     255 (Max MSP Parameter)
 uint8_t     0x01 (Parameter version 1)
 */
 #ifdef USE_AOA
-static void crsfFrameAoa(sbuf_t *dst)
+#define CRSF_AOA_MSP_SEQ_INCREMENT 1
+
+static void crsfFrameAoaMsp(sbuf_t *dst)
 {
     int16_t aoa = 0;
-    int16_t sideslip = 0;
-    aoaGetLatestData(&aoa, &sideslip);
+    int16_t unused = 0;
+    aoaGetLatestData(&aoa, &unused);
+
+    static uint8_t aoaMspSeq = 0;
+
+    uint8_t mspPayload[8];
+    mspPayload[0] = 0x50 | (aoaMspSeq & 0x0F);
+    mspPayload[1] = 0x00;
+    mspPayload[2] = 0x08;
+    mspPayload[3] = 0x1F;
+    mspPayload[4] = 0x02;
+    mspPayload[5] = 0x00;
+    mspPayload[6] = (uint8_t)(aoa & 0xFF);
+    mspPayload[7] = (uint8_t)((aoa >> 8) & 0xFF);
+
+    aoaMspSeq += CRSF_AOA_MSP_SEQ_INCREMENT;
 
-    sbufWriteU8(dst, CRSF_FRAME_AOA_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_AOA);
-    crsfSerialize16(dst, (uint16_t)aoa);
-    crsfSerialize16(dst, (uint16_t)sideslip);
+    sbufWriteU8(dst, sizeof(mspPayload) + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
+    crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    crsfSerializeData(dst, mspPayload, sizeof(mspPayload));
 }
 #endif
 
@@ -566,7 +583,7 @@ static void processCrsf(void)
 #ifdef USE_AOA
     if (currentSchedule & BV(CRSF_FRAME_AOA_INDEX)) {
         crsfInitializeFrame(dst);
-        crsfFrameAoa(dst);
+        crsfFrameAoaMsp(dst);
         crsfFinalize(dst);
     }
 #endif
@@ -690,8 +707,8 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
         crsfFrameVarioSensor(sbuf);
         break;
 #ifdef USE_AOA
-    case CRSF_FRAMETYPE_AOA:
-        crsfFrameAoa(sbuf);
+    case CRSF_FRAMETYPE_MSP_RESP:
+        crsfFrameAoaMsp(sbuf);
         break;
 #endif
     }