Add configurable faults for position or velocity tracking error

Author: jpieper

docs/protocol/registers.md

@@ -231,6 +231,10 @@ A fault code which will be set if the primary mode is 1 (Fault).
 * 48 - *invalid limits* - `servopos.position_min` or
   `servopos.position_max` are finite and outside the available
   position range
+* 49 - *position control error* - the position control error exceeded
+  the threshold configured in `servo.fault_position_error`
+* 50 - *velocity control error* - the velocity control error exceeded
+  the threshold configured in `servo.fault_velocity_error`
 
 Some non-zero codes can be presented during valid control modes
 without a fault.  These indicate which, if any, function is limiting

docs/reference/configuration.md

@@ -201,6 +201,20 @@ Torque begins to be limited when the motor temperature reaches this value.
 If the motor temperature reaches this value, a fault is triggered and
 all torque is stopped.
 
+## `servo.fault_position_error`
+
+If finite, a fault is triggered when the absolute position control
+error exceeds this value.  The position control error is the
+difference between the commanded and measured position in revolutions.
+If set to `nan` (the default), no fault is triggered.
+
+## `servo.fault_velocity_error`
+
+If finite, a fault is triggered when the absolute velocity control
+error exceeds this value.  The velocity control error is the
+difference between the commanded and measured velocity in revolutions
+per second.  If set to `nan` (the default), no fault is triggered.
+
 ## `servo.flux_brake_margin_voltage`
 
 Selects the flux braking point relative to the currently configured `servo.max_voltage`.  `flux braking point = max_voltage - flux_brake_margin_voltage`.

fw/bldc_servo_control.h

@@ -889,6 +889,21 @@ class BldcServoControl {
          feedforward_Nm +
          inertia_feedforward_Nm);
 
+    if (std::isfinite(self().config_.fault_position_error) &&
+        std::abs(self().status_.pid_position.error) >
+        self().config_.fault_position_error) {
+      self().status_.mode = kFault;
+      self().status_.fault = errc::kPositionControlError;
+      return;
+    }
+    if (std::isfinite(self().config_.fault_velocity_error) &&
+        std::abs(self().status_.pid_position.error_rate) >
+        self().config_.fault_velocity_error) {
+      self().status_.mode = kFault;
+      self().status_.fault = errc::kVelocityControlError;
+      return;
+    }
+
     const auto limited_torque_Nm_pair =
         LimitCode(unlimited_torque_Nm, -max_torque_Nm, max_torque_Nm,
                   errc::kLimitMaxTorque, errc::kSuccess);

fw/bldc_servo_structs.h

@@ -479,6 +479,9 @@ struct BldcServoConfig {
   float motor_temperature_margin = 20.0f;
   float motor_fault_temperature = std::numeric_limits<float>::quiet_NaN();
 
+  float fault_position_error = std::numeric_limits<float>::quiet_NaN();
+  float fault_velocity_error = std::numeric_limits<float>::quiet_NaN();
+
   float velocity_threshold = 0.0f;
   float position_derate = 0.02f;
 
@@ -615,6 +618,8 @@ struct BldcServoConfig {
     a->Visit(MJ_NVP(motor_thermistor_ohm));
     a->Visit(MJ_NVP(motor_temperature_margin));
     a->Visit(MJ_NVP(motor_fault_temperature));
+    a->Visit(MJ_NVP(fault_position_error));
+    a->Visit(MJ_NVP(fault_velocity_error));
     a->Visit(MJ_NVP(velocity_threshold));
     a->Visit(MJ_NVP(position_derate));
     a->Visit(MJ_NVP(adc_cur_cycles));

fw/error.h

@@ -50,6 +50,8 @@ enum class errc {
   kTimingViolation = 46,
   kBemfFeedforwardNoAccelLimit = 47,
   kInvalidLimits = 48,
+  kPositionControlError = 49,
+  kVelocityControlError = 50,
 
   // The following are not hard faults, but instead codes that are
   // presented while in a control mode if some factor is limiting the

fw/test/simulation_test.cc

@@ -371,4 +371,81 @@ BOOST_AUTO_TEST_CASE(SimVelocityAcrossBusVoltages) {
   }
 }
 
+// Test that position control error fault triggers when error exceeds threshold
+BOOST_AUTO_TEST_CASE(SimPositionControlErrorFault) {
+  ctx.config_.fault_position_error = 0.02f;
+
+  auto cmd = MakePositionCommand(0.0f, 0.0f, 1.0f);
+  ctx.Command(&cmd);
+
+  // Let it settle at position 0
+  ctx.RunSimulation(&cmd, 0.2f);
+  BOOST_TEST(ctx.status_.mode == kPosition);
+
+  // Apply a large external torque to push motor away from target
+  ctx.external_torque_ = 0.5f;
+
+  // Run until fault triggers
+  ctx.RunSimulation(&cmd, 1.0f);
+
+  BOOST_CHECK(ctx.status_.mode == kFault);
+  BOOST_CHECK(ctx.status_.fault == errc::kPositionControlError);
+}
+
+// Test that NaN position error threshold (default) does not trigger fault
+BOOST_AUTO_TEST_CASE(SimPositionControlErrorFaultDisabled) {
+  // fault_position_error defaults to NaN
+
+  auto cmd = MakePositionCommand(0.0f, 0.0f, 1.0f);
+  ctx.Command(&cmd);
+  ctx.RunSimulation(&cmd, 0.2f);
+
+  // Apply same external torque
+  ctx.external_torque_ = 0.5f;
+  ctx.RunSimulation(&cmd, 0.5f);
+
+  // Should NOT have faulted
+  BOOST_CHECK(ctx.status_.mode != kFault);
+}
+
+// Test that velocity control error fault triggers when error rate exceeds threshold
+BOOST_AUTO_TEST_CASE(SimVelocityControlErrorFault) {
+  ctx.config_.fault_velocity_error = 0.5f;
+
+  // Command constant velocity
+  auto cmd = MakePositionCommand(kNaN, 2.0f, 1.0f);
+  cmd.accel_limit = 10.0f;
+  ctx.Command(&cmd);
+
+  // Let it get up to speed
+  ctx.RunSimulation(&cmd, 0.5f);
+  BOOST_TEST(ctx.status_.mode == kPosition);
+
+  // Apply a large opposing torque to create velocity error
+  ctx.external_torque_ = -1.0f;
+
+  // Run until fault triggers
+  ctx.RunSimulation(&cmd, 1.0f);
+
+  BOOST_CHECK(ctx.status_.mode == kFault);
+  BOOST_CHECK(ctx.status_.fault == errc::kVelocityControlError);
+}
+
+// Test that NaN velocity error threshold (default) does not trigger fault
+BOOST_AUTO_TEST_CASE(SimVelocityControlErrorFaultDisabled) {
+  // fault_velocity_error defaults to NaN
+
+  auto cmd = MakePositionCommand(kNaN, 2.0f, 1.0f);
+  cmd.accel_limit = 10.0f;
+  ctx.Command(&cmd);
+  ctx.RunSimulation(&cmd, 0.5f);
+
+  // Apply opposing torque
+  ctx.external_torque_ = -1.0f;
+  ctx.RunSimulation(&cmd, 0.5f);
+
+  // Should NOT have faulted
+  BOOST_CHECK(ctx.status_.mode != kFault);
+}
+
 BOOST_AUTO_TEST_SUITE_END()

utils/gui/moteus_gui/tview.py

@@ -107,6 +107,8 @@
     46: "timing violation",
     47: "bemf feedforward without accel",
     48: "invalid limits",
+    49: "position control error",
+    50: "velocity control error",
     96: "limit: servo.max_velocity",
     97: "limit: servo.max_power_W",
     98: "limit: max system voltage",