Add an ability not to slow down when switching to the next waypoint

src/main/fc/settings.yaml

@@ -2288,6 +2288,11 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: nav_mc_wp_slowdown
+        description: "When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes."
+        default_value: "ON"
+        field: mc.slowDownForTurning
+        type: bool
       - name: nav_fw_cruise_thr
         description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
         default_value: "1400"

src/main/navigation/navigation.c

@@ -91,7 +91,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -144,6 +144,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .braking_bank_angle = 40,               // Max braking angle
         .posDecelerationTime = 120,             // posDecelerationTime * 100
         .posResponseExpo = 10,                  // posResponseExpo * 100
+        .slowDownForTurning = true,
     },
 
     // Fixed wing

src/main/navigation/navigation.h

@@ -219,6 +219,7 @@ typedef struct navConfig_s {
         uint8_t  braking_bank_angle;            // Max angle [deg] that MR is allowed duing braking boost phase
         uint8_t posDecelerationTime;            // Brake time parameter
         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
+        bool slowDownForTurning;             // Slow down during WP missions when changing heading on next waypoint
     } mc;
 
     struct {
@@ -250,7 +251,7 @@ typedef struct navConfig_s {
         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
         uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
         bool     allow_manual_thr_increase;
-        bool useFwNavYawControl;
+        bool    useFwNavYawControl;
         uint8_t yawControlDeadband;
     } fw;
 } navConfig_t;

src/main/navigation/navigation_multicopter.c

@@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
 static float getVelocityHeadingAttenuationFactor(void)
 {
     // In WP mode scale velocity if heading is different from bearing
-    if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
+    if (navConfig()->mc.slowDownForTurning && (navGetCurrentStateFlags() & NAV_AUTO_WP)) {
         const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
         const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));