feat(aoa): optimize canard intervention logic and add MSP over CRSF telemetry

Author: Miki4751

src/main/flight/pid.c

@@ -873,7 +873,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float
 
 #ifdef USE_AOA
     if (pidState->axis == FD_PITCH) {
-        aoaControlUpdate(axisPID[FD_PITCH], rateError, newPTerm, newDTerm, newFFTerm, pidState->errorGyroIf, limit);
+        aoaControlUpdate(&axisPID[FD_PITCH], rateError, newPTerm, newDTerm, newFFTerm, pidState->errorGyroIf, limit);
     }
 #endif
 

src/main/sensors/aoa.c

@@ -186,7 +186,7 @@ bool aoaControlEnable(int8_t input_rc_channel)
     return rcValue > 1666;
 }
 
-void aoaControlUpdate(float pidPitchOutput, float rateError, float newPTerm, float newDTerm, float newFFTerm, float errorGyroIf, float limit)
+void aoaControlUpdate(float *pidPitchOutput, float rateError, float newPTerm, float newDTerm, float newFFTerm, float errorGyroIf, float limit)
 {
     isAoaControlEnabled = aoaControlEnable(aoaControlConfig()->fw_aoa_control_channel);
 
@@ -202,22 +202,37 @@ void aoaControlUpdate(float pidPitchOutput, float rateError, float newPTerm, flo
     float aoaServoOffset = 0.0f;
 
     if (aoaControlConfig()->fw_aoa_aircraft_type == AIRCRAFT_CANARD) {
-        // Canard layout: always intervene with servo offset
+
+        const float thresholdRatio = aoaControlConfig()->fw_aoa_intervention_threshold * 0.01f;
+        const int16_t upperThreshold = upperLimitAngle * thresholdRatio;
+        const int16_t lowerThreshold = lowerLimitAngle * thresholdRatio;
+        const int16_t aoaDeg = DECIDEGREES_TO_DEGREES(filteredAoa);
+
         const int16_t lowerLimit = lowerLimitAngle * deg2pwm;
         const int16_t upperLimit = upperLimitAngle * deg2pwm;
+        float constrainedPidOutput = constrainf(*pidPitchOutput, -upperLimit, -lowerLimit);
 
-        float pidOutput = pidPitchOutput * kp;
-        float constrainedPidOutput = constrainf(pidOutput, -upperLimit, -lowerLimit);
+        float aoaError = 0.0f;
+        if (aoaDeg > upperThreshold) {
+            aoaError = aoaDeg - upperThreshold;
+        } else if (aoaDeg < lowerThreshold) {
+            aoaError = aoaDeg - lowerThreshold;
+        }
 
+        float interventionOffset = aoaError * kp * deg2pwm;
         aoaServoOffset = (DECIDEGREES_TO_DEGREES(filteredAoa) - aoaControlConfig()->fw_aoa_trim_angle) * deg2pwm;
-
-        aoaPidOutput = isAoaControlEnabled ? constrainedPidOutput + aoaServoOffset : constrainedPidOutput;
+        aoaPidOutput = isAoaControlEnabled ?  constrainedPidOutput + aoaServoOffset : constrainedPidOutput;
         aoaPidOutput = constrainf(aoaPidOutput, -limit, +limit);
 
-        DEBUG_SET(DEBUG_AOA, 1, pidOutput);
-        DEBUG_SET(DEBUG_AOA, 2, pidPitchOutput);
+        if (isAoaControlEnabled && aoaError != 0.0f) {
+            *pidPitchOutput += interventionOffset;
+            *pidPitchOutput = constrainf(*pidPitchOutput, -limit, +limit);
+        }
+
+        DEBUG_SET(DEBUG_AOA, 1, aoaError);
+        DEBUG_SET(DEBUG_AOA, 2, *pidPitchOutput);
         DEBUG_SET(DEBUG_AOA, 3, aoaControlConfig()->fw_aoa_kp);
-        DEBUG_SET(DEBUG_AOA, 4, constrainedPidOutput);
+        DEBUG_SET(DEBUG_AOA, 4, interventionOffset);
         DEBUG_SET(DEBUG_AOA, 5, aoaServoOffset);
         DEBUG_SET(DEBUG_AOA, 6, aoaPidOutput);
         DEBUG_SET(DEBUG_AOA, 7, rateError);
@@ -237,11 +252,11 @@ void aoaControlUpdate(float pidPitchOutput, float rateError, float newPTerm, flo
 
         aoaServoOffset = aoaError * kp * deg2pwm;
 
-        aoaPidOutput = isAoaControlEnabled ? pidPitchOutput + aoaServoOffset : pidPitchOutput;
+        aoaPidOutput = isAoaControlEnabled ? *pidPitchOutput + aoaServoOffset : *pidPitchOutput;
         aoaPidOutput = constrainf(aoaPidOutput, -limit, +limit);
 
         DEBUG_SET(DEBUG_AOA, 1, aoaError);
-        DEBUG_SET(DEBUG_AOA, 2, pidPitchOutput);
+        DEBUG_SET(DEBUG_AOA, 2, *pidPitchOutput);
         DEBUG_SET(DEBUG_AOA, 3, aoaControlConfig()->fw_aoa_kp);
         DEBUG_SET(DEBUG_AOA, 4, aoaServoOffset);
         DEBUG_SET(DEBUG_AOA, 5, aoaServoOffset);

src/main/sensors/aoa.h

@@ -82,4 +82,4 @@ timeDelta_t aoaUpdate(void);
 bool aoaProcess(void);
 bool aoaIsHealthy(void);
 bool aoaControlEnable(int8_t input_rc_channel);
-void aoaControlUpdate(float pidPitchOutput, float rateError, float newPTerm, float newDTerm, float newFFTerm, float errorGyroIf, float limit);
+void aoaControlUpdate(float *pidPitchOutput, float rateError, float newPTerm, float newDTerm, float newFFTerm, float errorGyroIf, float limit);

src/main/telemetry/crsf.c

@@ -57,6 +57,7 @@
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
+#include "sensors/aoa.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
@@ -434,6 +435,37 @@ uint32_t    Null Bytes
 uint8_t     255 (Max MSP Parameter)
 uint8_t     0x01 (Parameter version 1)
 */
+#ifdef USE_AOA
+#define CRSF_AOA_MSP_SEQ_INCREMENT 1
+
+static void crsfFrameAoaMsp(sbuf_t *dst)
+{
+    int16_t aoa = 0;
+    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, 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
+
 static void crsfFrameDeviceInfo(sbuf_t *dst)
 {
     char buff[30];
@@ -465,6 +497,9 @@ typedef enum {
     CRSF_FRAME_GPS_INDEX,
     CRSF_FRAME_VARIO_SENSOR_INDEX,
     CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
+#ifdef USE_AOA
+    CRSF_FRAME_AOA_INDEX,
+#endif
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -544,6 +579,13 @@ static void processCrsf(void)
         crsfBarometerAltitude(dst);
         crsfFinalize(dst);
     }
+#endif
+#ifdef USE_AOA
+    if (currentSchedule & BV(CRSF_FRAME_AOA_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameAoaMsp(dst);
+        crsfFinalize(dst);
+    }
 #endif
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -582,6 +624,11 @@ void initCrsfTelemetry(void)
     if (sensors(SENSOR_BARO)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
     }
+#endif
+#ifdef USE_AOA
+    if (sensors(SENSOR_AOA)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_AOA_INDEX);
+    }
 #endif
     crsfScheduleCount = (uint8_t)index;
 }
@@ -659,6 +706,11 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
     case CRSF_FRAMETYPE_VARIO_SENSOR:
         crsfFrameVarioSensor(sbuf);
         break;
+#ifdef USE_AOA
+    case CRSF_FRAMETYPE_MSP_RESP:
+        crsfFrameAoaMsp(sbuf);
+        break;
+#endif
     }
     const int frameSize = crsfFinalizeBuf(sbuf, frame);
     return frameSize;