Port Betaflight FPV camera tilt compensation for ACRO mode

src/main/fc/controlrate_profile.c

@@ -33,7 +33,7 @@
 const controlRateConfig_t *currentControlRateProfile;
 
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 2);
+PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3);
 
 void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
 {
@@ -61,6 +61,10 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
                 .rates[FD_ROLL] = 100,
                 .rates[FD_PITCH] = 100,
                 .rates[FD_YAW] = 100
+            },
+
+            .misc = {
+                .fpvCamAngleDegrees = 0
             }
         );
     }

src/main/fc/controlrate_profile.h

@@ -62,6 +62,10 @@ typedef struct controlRateConfig_s {
         uint8_t rates[3];
     } manual;
 
+    struct {
+        uint8_t fpvCamAngleDegrees;             // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis)
+    } misc;
+
 } controlRateConfig_t;
 
 PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

src/main/fc/fc_msp_box.c

@@ -67,6 +67,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXAIRMODE, "AIR MODE", 29 },
     { BOXHOMERESET, "HOME RESET", 30 },
     { BOXGCSNAV, "GCS NAV", 31 },
+    { BOXFPVANGLEMIX, "FPV ANGLE MIX", 32 },
     { BOXSURFACE, "SURFACE", 33 },
     { BOXFLAPERON, "FLAPERON", 34 },
     { BOXTURNASSIST, "TURN ASSIST", 35 },
@@ -178,6 +179,8 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
     }
 
+    activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
+
     //Camstab mode is enabled always
     activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
 
@@ -304,6 +307,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)),        BOXHEADFREE);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)),     BOXHEADADJ);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)),     BOXCAMSTAB);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX);
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)),          BOXMANUAL);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)),    BOXBEEPERON);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)),      BOXLEDLOW);

src/main/fc/rc_modes.h

@@ -65,6 +65,7 @@ typedef enum {
     BOXBRAKING      = 36,
     BOXUSER1        = 37,
     BOXUSER2        = 38,
+    BOXFPVANGLEMIX  = 39,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -733,6 +733,10 @@ groups:
         field: manual.rates[FD_YAW]
         min: 0
         max: 100
+      - name: fpv_mix_degrees
+        field: misc.fpvCamAngleDegrees
+        min: 0
+        max: 50
 
   - name: PG_SERIAL_CONFIG
     type: serialConfig_t

src/main/flight/pid.c

@@ -34,6 +34,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/pid.h"
@@ -753,8 +754,28 @@ static void pidTurnAssistant(pidState_t *pidState)
     }
 }
 
+static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAngle)
+{
+    static uint8_t lastFpvCamAngleDegrees = 0;
+    static float cosCameraAngle = 1.0;
+    static float sinCameraAngle = 0.0;
+
+    if (lastFpvCamAngleDegrees != fpvCameraAngle) {
+        lastFpvCamAngleDegrees = fpvCameraAngle;
+        cosCameraAngle = cos_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
+        sinCameraAngle = sin_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
+    }
+
+    // Rotate roll/yaw command from camera-frame coordinate system to body-frame coordinate system
+    const float rollRate = pidState[ROLL].rateTarget;
+    const float yawRate = pidState[YAW].rateTarget;
+    pidState[ROLL].rateTarget = constrainf(rollRate * cosCameraAngle -  yawRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
+    pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
+}
+
 void pidController(void)
 {
+    bool canUseFpvCameraMix = true;
     uint8_t headingHoldState = getHeadingHoldState();
 
     if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) {
@@ -783,10 +804,16 @@ void pidController(void)
         const float horizonRateMagnitude = calcHorizonRateMagnitude();
         pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude);
         pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude);
+        canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
         pidTurnAssistant(pidState);
+        canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
+    }
+
+    if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) {
+        pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
     }
 
     // Apply setpoint rate of change limits