slop

server/src/services/overcurrentProtectionService/index.js

@@ -6,15 +6,14 @@ const logger = require('../../globals/logger').child('overcurrentProtectionServi
 
 const DEFAULT_CONFIG = Object.freeze({
   minimumUsefulWheelIntent: 75,
-  // A fully stalled wheel now accumulates 0.25 stress per second, producing a
-  // roughly four-second hard-stop window. The smaller base rate also prevents
-  // a wheel that is still making progress from being treated like a hard jam.
   stressGrace: 0.25,
-  baseWheelOvercurrentRatePerSec: 0.08,
-  stalledWheelAdditionalRatePerSec: 0.17,
-  // Clear telemetry removes stress faster than even a complete stall adds it,
-  // so short threshold climbs and direction-change spikes do not linger.
-  wheelRecoveryRatePerSec: 0.5,
+  wheelProgressWindowSec: 0.4,
+  encoderNoiseFloorMmPerSec: 15,
+  fullStallProgressRatio: 0.1,
+  movingProgressRatio: 0.6,
+  movingOrUnknownRatePerSec: 0.25,
+  fullStallRatePerSec: 1,
+  wheelRecoveryRatePerSec: 0.75,
   brushOvercurrentRatePerSec: 1,
   brushRecoveryRatePerSec: 0.75,
   clearBeforeUnlockSec: 0.75,
@@ -41,6 +40,10 @@ function createMotorState() {
     measuredSpeed: null,
     currentMa: null,
     stallFactor: 0,
+    progressRatio: null,
+    classification: 'unknown',
+    progressSamples: [],
+    windowCommandSign: 0,
     stress: 0,
     cap: 1,
   };
@@ -240,23 +243,117 @@ function createOvercurrentProtectionService(options = {}) {
     return payload;
   }
 
-  function updateWheelMotor(motor, { overcurrent, command, measured, currentMa, deltaSec }) {
-    const commandMagnitude = Math.abs(finiteNumber(command));
-    const measuredNumber = Number(measured);
-    const measuredMagnitude = Number.isFinite(measuredNumber) ? Math.abs(measuredNumber) : null;
-    const usefulIntent = commandMagnitude >= config.minimumUsefulWheelIntent;
-    const motionRatio = usefulIntent && measuredMagnitude != null
-      ? clampUnit(measuredMagnitude / Math.max(commandMagnitude, config.minimumUsefulWheelIntent))
-      : 1;
-    const stallFactor = usefulIntent ? 1 - motionRatio : 0;
-    const riseRate = config.baseWheelOvercurrentRatePerSec
-      + config.stalledWheelAdditionalRatePerSec * stallFactor;
+  function resetWheelProgressWindow(motor, commandSign = 0) {
+    motor.progressSamples = [];
+    motor.windowCommandSign = commandSign;
+    motor.progressRatio = null;
+    motor.classification = 'unknown';
+    motor.stallFactor = 0;
+  }
+
+  function appendWheelProgressSample(motor, sample) {
+    motor.progressSamples.push(sample);
+    let windowSec = motor.progressSamples.reduce((total, entry) => total + entry.deltaSec, 0);
+
+    /*
+      Keep an actual rolling time window rather than periodically clearing a
+      bucket. If the oldest sample crosses the boundary, retain only its
+      proportional tail so classification does not depend on sensor cadence.
+    */
+    while (motor.progressSamples.length && windowSec > config.wheelProgressWindowSec) {
+      const excessSec = windowSec - config.wheelProgressWindowSec;
+      const oldest = motor.progressSamples[0];
+      if (oldest.deltaSec <= excessSec) {
+        motor.progressSamples.shift();
+        windowSec -= oldest.deltaSec;
+        continue;
+      }
+      const retainedFraction = (oldest.deltaSec - excessSec) / oldest.deltaSec;
+      oldest.deltaSec -= excessSec;
+      oldest.expectedDistance *= retainedFraction;
+      oldest.alignedDistance *= retainedFraction;
+      windowSec = config.wheelProgressWindowSec;
+    }
+    return windowSec;
+  }
+
+  function classifyWheelProgress(motor, windowSec) {
+    if (windowSec < config.wheelProgressWindowSec * 0.9) return;
+    const totals = motor.progressSamples.reduce(
+      (result, sample) => ({
+        expectedDistance: result.expectedDistance + sample.expectedDistance,
+        alignedDistance: result.alignedDistance + sample.alignedDistance,
+      }),
+      { expectedDistance: 0, alignedDistance: 0 },
+    );
+    if (totals.expectedDistance <= 0) return;
+
+    /*
+      Signed, command-aligned distance lets forward/backward encoder wobble
+      cancel over the window. The fixed noise floor then removes small residual
+      bias from gearbox lash or a wheel module rocking without real travel.
+    */
+    const averageExpectedSpeed = totals.expectedDistance / windowSec;
+    const averageAlignedSpeed = totals.alignedDistance / windowSec;
+    const usefulProgressSpeed = Math.max(0, averageAlignedSpeed - config.encoderNoiseFloorMmPerSec);
+    const progressRatio = clampUnit(usefulProgressSpeed / averageExpectedSpeed);
+    const ratioRange = Math.max(0.0001, config.movingProgressRatio - config.fullStallProgressRatio);
+    const stallFactor = clampUnit((config.movingProgressRatio - progressRatio) / ratioRange);
+
+    motor.progressRatio = progressRatio;
+    motor.stallFactor = stallFactor;
+    motor.classification = progressRatio <= config.fullStallProgressRatio
+      ? 'stalled'
+      : progressRatio >= config.movingProgressRatio
+        ? 'moving'
+        : 'partial';
+  }
+
+  function updateWheelMotor(motor, { overcurrent, command, intent, measured, currentMa, deltaSec }) {
+    const commandNumber = finiteNumber(command);
+    const commandMagnitude = Math.abs(commandNumber);
+    const commandSign = Math.sign(commandNumber);
+    const intentMagnitude = Math.abs(finiteNumber(intent));
+    // Null is intentionally checked before Number conversion because
+    // Number(null) is zero, which would falsely turn missing telemetry into a
+    // perfectly stalled wheel.
+    const measuredNumber = measured == null ? null : Number(measured);
+    const measuredValid = Number.isFinite(measuredNumber);
+    const usefulIntent = intentMagnitude >= config.minimumUsefulWheelIntent;
 
     motor.overcurrent = Boolean(overcurrent);
-    motor.commandedSpeed = finiteNumber(command);
-    motor.measuredSpeed = measuredMagnitude;
+    motor.commandedSpeed = commandNumber;
+    motor.measuredSpeed = measuredValid ? measuredNumber : null;
     motor.currentMa = Number.isFinite(Number(currentMa)) ? Number(currentMa) : null;
-    motor.stallFactor = stallFactor;
+
+    if (!motor.overcurrent) {
+      resetWheelProgressWindow(motor, commandSign);
+    } else if (!usefulIntent || commandMagnitude <= 0 || !measuredValid) {
+      // Low commands and missing telemetry are real overcurrent events, but
+      // neither supplies enough evidence to label the wheel mechanically stalled.
+      // Raw operator intent decides whether the classifier remains meaningful;
+      // the progressively smaller applied output must not erase an already
+      // confirmed stall merely because protection itself reduced it below the
+      // normal command threshold.
+      resetWheelProgressWindow(motor, commandSign);
+    } else {
+      if (motor.windowCommandSign !== commandSign) {
+        // Samples from opposite requested directions cannot share a progress
+        // window because their aligned distances describe different motion.
+        resetWheelProgressWindow(motor, commandSign);
+      }
+      if (deltaSec > 0) {
+        const windowSec = appendWheelProgressSample(motor, {
+          deltaSec,
+          expectedDistance: commandMagnitude * deltaSec,
+          alignedDistance: measuredNumber * commandSign * deltaSec,
+        });
+        classifyWheelProgress(motor, windowSec);
+      }
+    }
+
+    const riseRate = config.movingOrUnknownRatePerSec
+      + (config.fullStallRatePerSec - config.movingOrUnknownRatePerSec) * motor.stallFactor;
     motor.stress = clampUnit(
       motor.stress
         + (motor.overcurrent ? riseRate * deltaSec : -config.wheelRecoveryRatePerSec * deltaSec),
@@ -373,14 +470,16 @@ function createOvercurrentProtectionService(options = {}) {
 
     updateWheelMotor(state.motors.leftWheel, {
       overcurrent: flags.leftWheel,
-      command: state.driveIntent.left,
+      command: state.lastDriveOutput.left,
+      intent: state.driveIntent.left,
       measured: speeds.left,
       currentMa: sensors?.wheelLeftCurrentMa,
       deltaSec,
     });
     updateWheelMotor(state.motors.rightWheel, {
       overcurrent: flags.rightWheel,
-      command: state.driveIntent.right,
+      command: state.lastDriveOutput.right,
+      intent: state.driveIntent.right,
       measured: speeds.right,
       currentMa: sensors?.wheelRightCurrentMa,
       deltaSec,
@@ -432,7 +531,11 @@ function createOvercurrentProtectionService(options = {}) {
   function getPublicState(roverId) {
     const state = getState(roverId);
     const motors = MOTOR_KEYS.reduce((result, key) => {
-      result[key] = { ...state.motors[key] };
+      // Rolling samples are internal evidence, not UI state. Excluding them
+      // keeps every sensor frame compact while exposing the resulting ratio and
+      // classification needed to explain the service's decision.
+      const { progressSamples: _progressSamples, windowCommandSign: _windowCommandSign, ...motor } = state.motors[key];
+      result[key] = motor;
       return result;
     }, {});
     return {

server/src/services/overcurrentProtectionService/index.test.js

@@ -62,7 +62,7 @@ test('persistent stalled-wheel overcurrent scales both wheels and then stops dri
     driveDirect: { left: 300, right: 200 },
   });
 
-  for (let step = 0; step <= 39; step += 1) {
+  for (let step = 0; step <= 12; step += 1) {
     service.processTelemetry('rover', makeSensors({
       wheelOvercurrents: { leftWheel: true },
       wheelSpeedsMmPerSecond: { left: 0, right: 200 },
@@ -70,15 +70,15 @@ test('persistent stalled-wheel overcurrent scales both wheels and then stops dri
   }
 
   /*
-    Thirty-nine accumulated 100 ms intervals represent 3.9 seconds at the
-    maximum 0.25/s rate. The drive must still be available immediately before
-    the intended four-second hard-stop boundary.
+    The first 400 ms establish that the wheel is making no net progress. Once
+    classified, the faster stalled rate should approach—but not yet cross—the
+    hard-stop boundary at 1.2 seconds.
   */
   assert.equal(service.getPublicState('rover').drive.blocked, false);
   service.processTelemetry('rover', makeSensors({
     wheelOvercurrents: { leftWheel: true },
     wheelSpeedsMmPerSecond: { left: 0, right: 200 },
-  }), start + 4000);
+  }), start + 1300);
 
   const snapshot = service.getPublicState('rover');
   assert.equal(snapshot.status, 'stopped');
@@ -133,20 +133,129 @@ test('administrator commands and telemetry bypass all enforcement', () => {
   assert.equal(issued.length, 0);
 });
 
+test('encoder wobble around zero is classified as a full stall', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  for (let step = 0; step <= 13; step += 1) {
+    const wobbleSpeed = step % 2 === 0 ? 10 : -9;
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: wobbleSpeed },
+    }), start + step * 100);
+  }
+
+  const snapshot = service.getPublicState('rover');
+  assert.equal(snapshot.motors.leftWheel.classification, 'stalled');
+  assert.equal(snapshot.motors.leftWheel.stallFactor, 1);
+  assert.equal(snapshot.drive.blocked, true);
+});
+
+test('overcurrent while a wheel keeps moving accumulates at the slower rate', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  for (let step = 0; step <= 20; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: 240 },
+    }), start + step * 100);
+  }
+
+  const snapshot = service.getPublicState('rover');
+  assert.equal(snapshot.motors.leftWheel.classification, 'moving');
+  assert.equal(snapshot.motors.leftWheel.stallFactor, 0);
+  assert.equal(snapshot.drive.blocked, false);
+  assert.ok(snapshot.motors.leftWheel.stress < 0.6);
+});
+
+test('partial wheel progress produces an intermediate stall factor', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  for (let step = 0; step <= 5; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: 105 },
+    }), start + step * 100);
+  }
+
+  const motor = service.getPublicState('rover').motors.leftWheel;
+  assert.equal(motor.classification, 'partial');
+  assert.ok(motor.stallFactor > 0 && motor.stallFactor < 1);
+});
+
+test('missing wheel speed remains unknown instead of becoming a full stall', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  for (let step = 0; step <= 20; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: null },
+    }), start + step * 100);
+  }
+
+  const snapshot = service.getPublicState('rover');
+  assert.equal(snapshot.motors.leftWheel.measuredSpeed, null);
+  assert.equal(snapshot.motors.leftWheel.classification, 'unknown');
+  assert.equal(snapshot.motors.leftWheel.stallFactor, 0);
+  assert.equal(snapshot.drive.blocked, false);
+});
+
+test('wheel comparison follows scaled output and resets after reversal', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  for (let step = 0; step <= 6; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: 0 },
+    }), start + step * 100);
+  }
+  const scaled = service.getPublicState('rover').motors.leftWheel.commandedSpeed;
+  assert.ok(scaled < 300);
+
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: -300, right: -300 },
+  });
+  const reversed = service.processTelemetry('rover', makeSensors({
+    wheelOvercurrents: { leftWheel: true },
+    wheelSpeedsMmPerSecond: { left: -250 },
+  }), start + 700);
+  assert.equal(reversed.motors.leftWheel.classification, 'unknown');
+  assert.equal(reversed.motors.leftWheel.progressRatio, null);
+});
+
 test('a stopped drive stays blocked until both clear time and neutral are observed', () => {
   const { service } = createHarness();
   const start = Date.now();
   service.protectCommand('rover', 'drive', {
     driveDirect: { left: 300, right: 300 },
   });
-  for (let step = 0; step <= 40; step += 1) {
+  for (let step = 0; step <= 13; step += 1) {
     service.processTelemetry('rover', makeSensors({
       wheelOvercurrents: { leftWheel: true },
       wheelSpeedsMmPerSecond: { left: 0 },
     }), start + step * 100);
   }
 
-  for (let step = 41; step <= 61; step += 1) {
+  for (let step = 14; step <= 28; step += 1) {
     service.processTelemetry('rover', makeSensors(), start + step * 100);
   }
   assert.equal(service.getPublicState('rover').drive.blocked, true);