Use TURN_ASSIST for navigation

src/main/flight/pid.c

@@ -699,7 +699,7 @@ void pidController(void)
     }
 
 #ifdef USE_FLM_TURN_ASSIST
-    if (FLIGHT_MODE(TURN_ASSISTANT)) {
+    if (FLIGHT_MODE(TURN_ASSISTANT) || naivationRequiresTurnAssistance()) {
         pidTurnAssistant(pidState);
     }
 #endif

src/main/navigation/navigation.c

@@ -2420,13 +2420,33 @@ bool naivationRequiresAngleMode(void)
     return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
 }
 
+/*-----------------------------------------------------------
+ * An indicator that TURN ASSISTANCE is required for navigation
+ *-----------------------------------------------------------*/
+bool naivationRequiresTurnAssistance(void)
+{
+    const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
+    if (STATE(FIXED_WING)) {
+        // For airplanes turn assistant is always required when controlling position
+        return (currentState & NAV_CTL_POS);
+    }
+    else {
+        return false;
+    }
+}
+
 /**
  * An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
  */
 int8_t naivationGetHeadingControlState(void)
 {
-    // No explicit MAG_HOLD mode for airplanes
-    if ((navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) && !STATE(FIXED_WING)) {
+    // For airplanes report as manual heading control
+    if (STATE(FIXED_WING)) {
+        return NAV_HEADING_CONTROL_MANUAL;
+    }
+
+    // For multirotors it depends on navigation system mode
+    if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
         if (posControl.flags.isAdjustingHeading) {
             return NAV_HEADING_CONTROL_MANUAL;
         }

src/main/navigation/navigation.h

@@ -258,6 +258,7 @@ void applyWaypointNavigationAndAltitudeHold(void);
 /* Functions to signal navigation requirements to main loop */
 bool naivationRequiresAngleMode(void);
 bool navigationRequiresThrottleTiltCompensation(void);
+bool naivationRequiresTurnAssistance(void);
 int8_t naivationGetHeadingControlState(void);
 bool naivationBlockArming(void);
 bool navigationPositionEstimateIsHealthy(void);

src/main/navigation/navigation_fixedwing.c

@@ -297,13 +297,6 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
-
-    // Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
-    posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
-    updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
-
-    // Add pitch compensation
-    //posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
 }
 
 void applyFixedWingPositionController(timeUs_t currentTimeUs)
@@ -440,15 +433,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
     if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
         rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
-
-        // Calculate coordinated turn rate based on velocity and banking angle
-        if (posControl.actualState.velXY >= 300.0f) {
-            float targetYawRateDPS = RADIANS_TO_DEGREES(tan_approx(DECIDEGREES_TO_RADIANS(-rollCorrection)) * GRAVITY_CMSS / posControl.actualState.velXY);
-            rcCommand[YAW] = pidRateToRcCommand(targetYawRateDPS, currentControlRateProfile->rates[FD_YAW]);
-        }
-        else {
-            rcCommand[YAW] = 0;
-        }
     }
 
     if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {

src/main/target/common.h

@@ -36,9 +36,9 @@
 #define GPS
 #define GPS_PROTO_UBLOX
 #define NAV
+#define USE_FLM_TURN_ASSIST     // This is mandatory for fixed-wing navigation
 #define TELEMETRY
 #define TELEMETRY_LTM
-#define USE_FLM_TURN_ASSIST
 #define TELEMETRY_FRSKY
 #endif