Velocity attenuation for WP mode if not facing the next waypoint. Turn first, accelerate later logic. Don't apply rth_tail_first for airplanes - can't go tail first

src/main/flight/navigation_rewrite.c

@@ -602,6 +602,11 @@ static uint32_t navGetCurrentStateTime(void)
     return millis() - posControl.navStateActivationTimeMs;
 }
 
+navigationFSMStateFlags_t navGetCurrentStateFlags(void)
+{
+    return navGetStateFlags(posControl.navState);
+}
+
 /*************************************************************************************************/
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
 {
@@ -742,7 +747,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
     }
     else {
         // Update XY-position target
-        if (posControl.navConfig->flags.rth_tail_first) {
+        if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
             setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
         }
         else {
@@ -825,8 +830,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
     if ((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) {
         return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_RTH_3D_HEAD_HOME
     }
-
+    else {
-    if (posControl.navConfig->flags.rth_tail_first) {
+        // Climb to safe altitude and turn to correct direction
+        if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
             setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
         }
         else {
@@ -834,6 +840,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
         }
 
         return NAV_FSM_EVENT_NONE;
+    }
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState)
@@ -851,7 +858,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
     }
     else {
         // Update XYZ-position target
-        if (posControl.navConfig->flags.rth_tail_first) {
+        if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
             setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
         }
         else {

src/main/flight/navigation_rewrite_multicopter.c

@@ -246,22 +246,39 @@ bool adjustMulticopterPositionFromRCInput(void)
     }
 }
 
+static float getVelocityAttenuationFactor(void)
+{
+    // In WP mode scale velocity if heading is different from bearing
+    if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
+        int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
+        float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));
+
+        return constrainf(velScaling * velScaling, 0.05f, 1.0f);
+    } else {
+        return 1.0f;
+    }
+}
+
 static void updatePositionVelocityController_MC(void)
 {
     float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
     float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
 
+    // Calculate target velocity
     float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
     float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
-    float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
 
+    // Scale velocity to respect max_speed
+    float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
     if (newVelTotal > posControl.navConfig->max_speed) {
         newVelX = posControl.navConfig->max_speed * (newVelX / newVelTotal);
         newVelY = posControl.navConfig->max_speed * (newVelY / newVelTotal);
     }
 
-    posControl.desiredState.vel.V.X = newVelX;
+    // Apply attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
-    posControl.desiredState.vel.V.Y = newVelY;
+    float velAttFactor = getVelocityAttenuationFactor();
+    posControl.desiredState.vel.V.X = newVelX * velAttFactor;
+    posControl.desiredState.vel.V.Y = newVelY * velAttFactor;
 
 #if defined(NAV_BLACKBOX)
     navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.V.X), -32678, 32767);

src/main/flight/navigation_rewrite_private.h

@@ -272,10 +272,14 @@ uint32_t calculateDistanceToDestination(t_fp_vector * destinationPos);
 int32_t calculateBearingToDestination(t_fp_vector * destinationPos);
 void resetLandingDetector(void);
 bool isLandingDetected(void);
+
+navigationFSMStateFlags_t navGetCurrentStateFlags(void);
+
 void setHomePosition(t_fp_vector * pos, int32_t yaw);
 void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
 void setDesiredSurfaceOffset(float surfaceOffset);
 void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
+
 bool isWaypointReached(navWaypointPosition_t * waypoint);
 bool isWaypointMissed(navWaypointPosition_t * waypoint);
 bool isApproachingLastWaypoint(void);