boost factor scaled to current speed

iNavFlight · May 12, 2018 · d363dc5 · d363dc5
1 parent 1fb1cf8
commit d363dc5
Showing 1 changed file with 23 additions and 7 deletions.
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
@@ -357,8 +357,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     if (velErrorMagnitude > 0.1f) {
         accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
         accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
-    }
-    else {
+    } else {
         accelLimitX = maxAccelLimit / 1.414213f;
         accelLimitY = accelLimitX;
     }
@@ -368,7 +367,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     float maxAccelChange = US2S(deltaMicros) * 1700.0f;
     //When braking, raise jerk limit even if we are not boosting acceleration
-    if (STATE(NAV_CRUISE_BRAKING)) {    
+    if (STATE(NAV_CRUISE_BRAKING)) {
         maxAccelChange = maxAccelChange * 2;
     }
 
@@ -389,9 +388,24 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     uint8_t accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ;
 
     //Boost required accelerations
-    if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0) {
-
-        const float boostFactor = (100.0f + (float) navConfig()->mc.braking_boost_factor) / 100.0f;
+    if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0)
+    {
+        const float rawBoostFactor = (100.0f + (float)navConfig()->mc.braking_boost_factor) / 100.0f;
+
+        //Scale boost factor according to speed
+        const float boostFactor = constrainf(
+            scaleRangef(
+                posControl.actualState.velXY, 
+                navConfig()->mc.braking_boost_speed_threshold, 
+                navConfig()->general.max_manual_speed, 
+                0, 
+                rawBoostFactor
+            ),
+            0, 
+            rawBoostFactor
+        );
+
+        DEBUG_SET(DEBUG_BRAKING_ACC, 3, boostFactor);
 
         //Boost required acceleration for harder braking
         newAccelX = newAccelX * boostFactor;
@@ -402,6 +416,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
         maxBankAngle = maxBankAngle * 120 / 100;
 
         accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ * 2;
+    } else {
+        DEBUG_SET(DEBUG_BRAKING_ACC, 3, 0);
     }
 
     // Save last acceleration target
@@ -414,7 +430,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     DEBUG_SET(DEBUG_BRAKING_ACC, 0, STATE(NAV_CRUISE_BRAKING_BOOST));
     DEBUG_SET(DEBUG_BRAKING_ACC, 1, accelN);
-    DEBUG_SET(DEBUG_BRAKING_ACC, 2, accelN);
+    DEBUG_SET(DEBUG_BRAKING_ACC, 2, accelE);
 
     // Rotate acceleration target into forward-right frame (aircraft)
     const float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;