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

docs/Settings.md

@@ -330,6 +330,7 @@
 | nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
+| nav_mc_wp_slowdown | ON | 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. |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
 | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |

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));