PC now "uses" the 6DOF features

src/main/flight/mixer.c

@@ -57,6 +57,8 @@ FILE_COMPILE_FOR_SPEED
 
 #include "sensors/battery.h"
 
+FASTRAM int16_t mixerForwardCommand = 0;
+FASTRAM int16_t mixerLateralCommand = 0;
 FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
 FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 static float motorMixRange;
@@ -150,6 +152,26 @@ static void computeMotorCount(void)
     }
 }
 
+bool mixerHasForwardAuthority(void) {
+    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+        const motorMixer_t *m = primaryMotorMixer(i);
+        if (m->forward != 0) {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool mixerHasLateralAuthority(void) {
+    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+        const motorMixer_t *m = primaryMotorMixer(i);
+        if (m->lateral != 0) {
+            return true;
+        }
+    }
+    return false;
+}
+
 uint8_t getMotorCount(void) {
     return motorCount;
 }
@@ -539,18 +561,24 @@ void FAST_CODE mixTable(const float dT)
     }
 #endif
 
-    int16_t input[3];   // RPY, range [-500:+500]
+    int16_t input[5];   // RPYFL, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
     if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
         // Direct passthru from RX
         input[ROLL] = rcCommand[ROLL];
         input[PITCH] = rcCommand[PITCH];
         input[YAW] = rcCommand[YAW];
+        // Forward/lateral aren't used in passthrough
+        // TODO figure out where to make FORWARD and LATERAL enums
+        input[3] = 0;
+        input[4] = 0;
     }
     else {
         input[ROLL] = axisPID[ROLL];
         input[PITCH] = axisPID[PITCH];
         input[YAW] = axisPID[YAW];
+        input[3] = mixerForwardCommand;
+        input[4] = mixerLateralCommand;
     }
 
     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
@@ -563,7 +591,9 @@ void FAST_CODE mixTable(const float dT)
         rpyMix[i] =
             (input[PITCH] * currentMixer[i].pitch +
             input[ROLL] * currentMixer[i].roll +
-            -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
+            -motorYawMultiplier * input[YAW] * currentMixer[i].yaw +
+            input[3] * currentMixer[i].forward +
+            input[4] * currentMixer[i].lateral) * mixerScale;
 
         if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
         if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];

src/main/flight/mixer.h

@@ -114,12 +114,16 @@ typedef enum {
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;
+extern int16_t mixerForwardCommand;
+extern int16_t mixerLateralCommand;
 
 int getThrottleIdleValue(void);
 int16_t getThrottlePercent(void);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);
+bool mixerHasForwardAuthority(void);
+bool mixerHasLateralAuthority(void);
 motorStatus_e getMotorStatus(void);
 
 void writeAllMotors(int16_t mc);

src/main/navigation/navigation_multicopter.c

@@ -621,8 +621,18 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
     const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
 
-    posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
-    posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
+    // Decide whether to get the requested acceleration from roll/pitch or directly from actuators
+    if (mixerHasForwardAuthority()) {
+        mixerForwardCommand = accelForward;
+    } else {
+        posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
+    }
+    if (mixerHasLateralAuthority()) {
+        mixerLateralCommand = accelRight;
+    } else {
+        posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
+    }
+
 }
 
 static void applyMulticopterPositionController(timeUs_t currentTimeUs)