Configurable limitation of max climb/descent rate (#257)

* ALTHOLD: Configurable limitation of max climb/descent rate
* ALTHOLD: Ignore the vertical acceleration limit when stopping (abs target vel < abs actual vel)

src/main/config/config.c

@@ -269,6 +269,7 @@ void resetNavConfig(navConfig_t * navConfig)
     navConfig->pos_failure_timeout = 5;     // 5 sec
     navConfig->waypoint_radius = 100;       // 2m diameter
     navConfig->max_speed = 300;             // 3 m/s = 10.8 km/h
+    navConfig->max_climb_rate = 500;        // 5 m/s
     navConfig->max_manual_speed = 500;
     navConfig->max_manual_climb_rate = 200;
     navConfig->land_descent_rate = 200;     // 2 m/s

src/main/flight/navigation_rewrite.h

@@ -104,6 +104,7 @@ typedef struct navConfig_s {
     uint8_t  pos_failure_timeout;           // Time to wait before switching to emergency landing (0 - disable)
     uint16_t waypoint_radius;               // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
     uint16_t max_speed;                     // autonomous navigation speed cm/sec
+    uint16_t max_climb_rate;                // max vertical speed limitation cm/sec
     uint16_t max_manual_speed;              // manual velocity control max horizontal speed
     uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
     uint16_t land_descent_rate;             // normal RTH landing descent rate

src/main/flight/navigation_rewrite_multicopter.c

@@ -87,12 +87,18 @@ static void updateAltitudeVelocityController_MC(uint32_t deltaMicros)
     float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
     float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
 
-    // hard limit desired target velocity to +/- 20 m/s
+    // hard limit desired target velocity to max_climb_rate
-    targetVel = constrainf(targetVel, -2000.0f, 2000.0f);
+    targetVel = constrainf(targetVel, -posControl.navConfig->max_climb_rate, posControl.navConfig->max_climb_rate);
 
-    // limit max vertical acceleration 250 cm/s/s - reach the max 20 m/s target in 80 seconds
+    // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
-    float maxVelDifference = US2S(deltaMicros) * 250.0f;
+    // if we are decelerating - don't limit (allow better recovery from falling)
-    posControl.desiredState.vel.V.Z = constrainf(targetVel, posControl.desiredState.vel.V.Z - maxVelDifference, posControl.desiredState.vel.V.Z + maxVelDifference);
+    if (ABS(targetVel) > ABS(posControl.desiredState.vel.V.Z)) {
+        float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f);
+        posControl.desiredState.vel.V.Z = constrainf(targetVel, posControl.desiredState.vel.V.Z - maxVelDifference, posControl.desiredState.vel.V.Z + maxVelDifference);
+    }
+    else {
+        posControl.desiredState.vel.V.Z = targetVel;
+    }
 
 #if defined(NAV_BLACKBOX)
     navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.V.Z), -32678, 32767);

src/main/io/serial_cli.c

@@ -621,6 +621,7 @@ const clivalue_t valueTable[] = {
     { "nav_position_timeout",       VAR_UINT8  | MASTER_VALUE, &masterConfig.navConfig.pos_failure_timeout, .config.minmax = { 0,  10 }, 0 },
     { "nav_wp_radius",              VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.waypoint_radius, .config.minmax = { 10,  10000 }, 0 },
     { "nav_max_speed",              VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_speed, .config.minmax = { 10,  2000 }, 0 },
+    { "nav_max_climb_rate",         VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_climb_rate, .config.minmax = { 10,  2000 }, 0 },
     { "nav_manual_speed",           VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_manual_speed, .config.minmax = { 10,  2000 }, 0 },
     { "nav_manual_climb_rate",      VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_manual_climb_rate, .config.minmax = { 10,  2000 }, 0 },
     { "nav_landing_speed",          VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.land_descent_rate, .config.minmax = { 100,  2000 }, 0 },