Improve the interaction between servo.velocity_limit and servo.max_ve…

…locity

docs/reference.md

@@ -1095,6 +1095,9 @@ limits are as follows:
   case, except that the control velocity is allowed to increase or
   decrease arbitrarily.
 
+NOTE: This is limited internally to be no more than
+`servo.max_velocity`.
+
 ## `servo.voltage_mode_control` ##
 
 When set to non-zero, the current control loop is not closed, and all
@@ -1192,10 +1195,6 @@ beyond the factory configured value can result in hardware damage.
 
 Output power will be limited if the velocity exceeds this threshold.
 
-NOTE: If this value is lower than the configured
-`servo.velocity_limit`, then the resulting behavior is unlikely to be
-useful.
-
 ## `servo.max_velocity_derate` ##
 
 Once velocity reaches the max_velocity plus this value, allowed output

fw/bldc_servo.cc

@@ -383,6 +383,17 @@ class BldcServo::Impl {
     if (std::isnan(next->accel_limit)) {
       next->accel_limit = config_.default_accel_limit;
     }
+    // If we are going to limit at all, ensure that we have a velocity
+    // limit, and that is is no more than the configured maximum
+    // velocity.
+    if (!std::isnan(next->velocity_limit) || !std::isnan(next->accel_limit)) {
+      if (std::isnan(next->velocity_limit)) {
+        next->velocity_limit = config_.max_velocity;
+      } else {
+        next->velocity_limit =
+            std::min(next->velocity_limit, config_.max_velocity);
+      }
+    }
 
     telemetry_data_ = *next;