Make the action to take during a position timeout configurable

docs/reference.md

@@ -1162,6 +1162,15 @@ When in the "position timeout" mode the controller acts to damp the
 output.  This parameter controls the maximum torque available for such
 damping.
 
+## `servo.timeout_mode` ##
+
+Selects what behavior will take place in the position timeout mode.
+The allowable values are a subset of the top level modes.
+
+* 0 - "stopped" - the driver is disengaged
+* 12 - "zero velocity"
+* 15 - "brake"
+
 ## `servo.rezero_from_abs` ##
 
 If set to one, then shortly after startup, the value of the position

fw/bldc_servo.cc

@@ -1063,13 +1063,15 @@ class BldcServo::Impl {
       case kVoltageDq:
       case kCurrent:
       case kPosition:
-      case kPositionTimeout:
       case kZeroVelocity:
       case kStayWithinBounds:
       case kMeasureInductance:
       case kBrake: {
         return true;
       }
+      case kPositionTimeout: {
+        return config_.timeout_mode != 0;
+      }
     }
     return false;
   }
@@ -1380,7 +1382,10 @@ class BldcServo::Impl {
         ISR_DoPosition(sin_cos, data);
         break;
       }
-      case kPositionTimeout:
+      case kPositionTimeout: {
+        ISR_DoPositionTimeout(sin_cos, data);
+        break;
+      }
       case kZeroVelocity: {
         ISR_DoZeroVelocity(sin_cos, data);
         break;
@@ -1708,6 +1713,18 @@ class BldcServo::Impl {
     ISR_DoBalancedVoltageControl(ISR_CalculatePhaseVoltage(sin_cos, d_V, q_V));
   }
 
+  void ISR_DoPositionTimeout(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
+    if (config_.timeout_mode == kStopped) {
+      ISR_DoStopped();
+    } else if (config_.timeout_mode == kZeroVelocity) {
+      ISR_DoZeroVelocity(sin_cos, data);
+    } else if (config_.timeout_mode == kBrake) {
+      ISR_DoBrake();
+    } else {
+      ISR_DoStopped();
+    }
+  }
+
   void ISR_DoZeroVelocity(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
     PID::ApplyOptions apply_options;
     apply_options.kp_scale = 0.0f;

fw/bldc_servo.h

@@ -196,6 +196,14 @@ class BldcServo {
     float default_timeout_s = 0.1f;
     float timeout_max_torque_Nm = 5.0f;
 
+    // Selects the behavior when in the timeout mode.  The available
+    // options map to top level modes, although only the following are
+    // valid:
+    //  0 - "stopped" - motor driver disengaged
+    //  12 - "zero velocity" - derivative only position control
+    //  15 - "brake" - all motor phases shorted to ground
+    uint8_t timeout_mode = 12;
+
     // Similar to 'max_voltage', the flux braking default voltage is
     // board rev dependent.
     float flux_brake_min_voltage = (g_measured_hw_rev <= 5) ? 34.5f : 43.5f;
@@ -272,6 +280,7 @@ class BldcServo {
       a->Visit(MJ_NVP(max_position_slip));
       a->Visit(MJ_NVP(default_timeout_s));
       a->Visit(MJ_NVP(timeout_max_torque_Nm));
+      a->Visit(MJ_NVP(timeout_mode));
       a->Visit(MJ_NVP(flux_brake_min_voltage));
       a->Visit(MJ_NVP(flux_brake_resistance_ohm));
       a->Visit(MJ_NVP(max_current_A));
@@ -345,14 +354,14 @@ class BldcServo {
 
     // This state can be commanded directly, and will also be entered
     // automatically upon a watchdog timeout from kPosition.  When in
-    // this state, the controller will apply a derivative only
-    // position control to slowly bring the servos to a resting
-    // position.
+    // this state, the controller will apply the selected fallback
+    // control mode.
     //
     // The only way to exit this state is through a stop command.
     kPositionTimeout = 11,
 
-    // This is just like kPositionTimeout, but is not latching.
+    // Control to zero velocity through a derivative only version of
+    // the position mode.
     kZeroVelocity = 12,
 
     // This applies the PID controller only to stay within a