slopcurrent

server/src/services/overcurrentProtectionService/index.js

@@ -0,0 +1,463 @@
+// Overcurrent Protection Service
+// Purpose: Owns fleet-wide, server-authoritative motor stress calculation and command limiting.
+// Scope: Keeps overcurrent policy out of rover-manager sensor orchestration while combining command intent with decoded telemetry.
+
+const logger = require('../../globals/logger').child('overcurrentProtectionService');
+
+const DEFAULT_CONFIG = Object.freeze({
+  minimumUsefulWheelIntent: 75,
+  stressGrace: 0.2,
+  baseWheelOvercurrentRatePerSec: 0.75,
+  stalledWheelAdditionalRatePerSec: 1.25,
+  wheelRecoveryRatePerSec: 1,
+  brushOvercurrentRatePerSec: 1,
+  brushRecoveryRatePerSec: 0.75,
+  clearBeforeUnlockSec: 0.75,
+  outputRateMs: 250,
+  maxTelemetryDeltaSec: 0.5,
+});
+
+const MOTOR_KEYS = Object.freeze(['leftWheel', 'rightWheel', 'mainBrush', 'sideBrush']);
+
+function clampUnit(value) {
+  if (!Number.isFinite(value)) return 0;
+  return Math.max(0, Math.min(1, value));
+}
+
+function finiteNumber(value, fallback = 0) {
+  const number = Number(value);
+  return Number.isFinite(number) ? number : fallback;
+}
+
+function createMotorState() {
+  return {
+    overcurrent: false,
+    commandedSpeed: 0,
+    measuredSpeed: null,
+    currentMa: null,
+    stallFactor: 0,
+    stress: 0,
+    cap: 1,
+  };
+}
+
+function createRoverState() {
+  return {
+    bypassed: false,
+    lastTelemetryAt: 0,
+    driveIntent: { left: 0, right: 0 },
+    auxIntent: { main: 0, side: 0, vacuum: 0 },
+    driveBlocked: false,
+    requiresNeutral: false,
+    neutralSeen: false,
+    driveClearSec: 0,
+    stopReason: null,
+    lastDriveOutputAt: 0,
+    lastAuxOutputAt: 0,
+    lastDriveOutput: { left: 0, right: 0 },
+    lastAuxOutput: { main: 0, side: 0, vacuum: 0 },
+    motors: {
+      leftWheel: createMotorState(),
+      rightWheel: createMotorState(),
+      mainBrush: createMotorState(),
+      sideBrush: createMotorState(),
+    },
+  };
+}
+
+function createOvercurrentProtectionService(options = {}) {
+  const config = Object.freeze({ ...DEFAULT_CONFIG, ...(options.config || {}) });
+  const states = new Map();
+  let commandIssuer = typeof options.issueCommand === 'function' ? options.issueCommand : null;
+
+  function getState(roverId) {
+    const key = String(roverId || '');
+    if (!states.has(key)) states.set(key, createRoverState());
+    return states.get(key);
+  }
+
+  function resetProtectionState(state, { bypassed = state.bypassed } = {}) {
+    /*
+      An administrator bypass is a change of authority, not merely a cap of
+      one. Clearing accumulated stress here prevents a previous user's event
+      from unexpectedly affecting the first command after authority changes.
+      Both command intents are cleared before the caller records the new command
+      so telemetry cannot apply a previous operator's held drive or brush value
+      after authority changes.
+    */
+    state.bypassed = Boolean(bypassed);
+    state.lastTelemetryAt = 0;
+    state.driveBlocked = false;
+    state.requiresNeutral = false;
+    state.neutralSeen = false;
+    state.driveClearSec = 0;
+    state.stopReason = null;
+    state.driveIntent = { left: 0, right: 0 };
+    state.auxIntent = { main: 0, side: 0, vacuum: 0 };
+    state.lastDriveOutput = { left: 0, right: 0 };
+    state.lastAuxOutput = { main: 0, side: 0, vacuum: 0 };
+    state.lastDriveOutputAt = 0;
+    state.lastAuxOutputAt = 0;
+    MOTOR_KEYS.forEach((key) => {
+      state.motors[key] = createMotorState();
+    });
+  }
+
+  function configureCommandIssuer(nextIssuer) {
+    commandIssuer = typeof nextIssuer === 'function' ? nextIssuer : null;
+  }
+
+  function calculateCap(stress) {
+    /*
+      The grace region absorbs short mechanical events such as initial wheel
+      acceleration and direction changes. Above it, the remaining stress range
+      maps linearly to output so the cap reaches exactly zero at hard-stop
+      stress instead of leaving a small command applied to a stalled motor.
+    */
+    const grace = clampUnit(config.stressGrace);
+    if (stress <= grace) return 1;
+    const usableRange = Math.max(0.0001, 1 - grace);
+    return clampUnit(1 - (stress - grace) / usableRange);
+  }
+
+  function getDriveCap(state) {
+    return Math.min(state.motors.leftWheel.cap, state.motors.rightWheel.cap);
+  }
+
+  function scaleDrive(state, driveDirect = state.driveIntent) {
+    if (state.bypassed) return { ...driveDirect };
+    if (state.driveBlocked) return { left: 0, right: 0 };
+    const cap = getDriveCap(state);
+    return {
+      left: Math.round(finiteNumber(driveDirect?.left) * cap),
+      right: Math.round(finiteNumber(driveDirect?.right) * cap),
+    };
+  }
+
+  function scaleAux(state, motorPwm = state.auxIntent) {
+    if (state.bypassed) return { ...motorPwm };
+    return {
+      main: Math.round(finiteNumber(motorPwm?.main) * state.motors.mainBrush.cap),
+      side: Math.round(finiteNumber(motorPwm?.side) * state.motors.sideBrush.cap),
+      // Create 2/Roomba 600 does not expose a vacuum overcurrent bit, so this
+      // service must not imply that it can measure or limit vacuum motor stress.
+      vacuum: Math.round(finiteNumber(motorPwm?.vacuum)),
+    };
+  }
+
+  function hasDriveIntent(state) {
+    return Boolean(state.driveIntent.left || state.driveIntent.right);
+  }
+
+  function hasAuxIntent(state) {
+    return Boolean(state.auxIntent.main || state.auxIntent.side || state.auxIntent.vacuum);
+  }
+
+  function driveOutputsEqual(left, right) {
+    return left.left === right.left && left.right === right.right;
+  }
+
+  function auxOutputsEqual(left, right) {
+    return left.main === right.main && left.side === right.side && left.vacuum === right.vacuum;
+  }
+
+  function issueAdjustedCommand(roverId, payload) {
+    if (!commandIssuer) return false;
+    try {
+      /*
+        The injected issuer is the raw server-to-roverd transport function.
+        Calling it here intentionally avoids routing a service-generated update
+        through the socket authorization/filter path a second time.
+      */
+      return commandIssuer(roverId, payload) !== false;
+    } catch (err) {
+      logger.warn('Failed to issue overcurrent protection command', {
+        roverId,
+        type: payload?.type,
+        error: err.message,
+      });
+      return false;
+    }
+  }
+
+  function protectCommand(roverId, type, payload = {}, context = {}) {
+    const state = getState(roverId);
+    const bypassed = Boolean(context.bypassed);
+    if (bypassed !== state.bypassed) {
+      resetProtectionState(state, { bypassed });
+    }
+
+    if (type === 'drive' && payload?.driveDirect) {
+      state.driveIntent = {
+        left: finiteNumber(payload.driveDirect.left),
+        right: finiteNumber(payload.driveDirect.right),
+      };
+
+      if (bypassed) {
+        state.lastDriveOutput = { ...state.driveIntent };
+        state.lastDriveOutputAt = Date.now();
+        return { ...payload, driveDirect: { ...state.driveIntent } };
+      }
+
+      const neutral = !state.driveIntent.left && !state.driveIntent.right;
+      if (neutral) {
+        /*
+          A real neutral command proves the operator released their controls.
+          Merely observing zero encoder motion cannot provide that assurance,
+          because a held command against an obstruction also produces no motion.
+        */
+        state.neutralSeen = true;
+        if (state.driveBlocked && state.driveClearSec >= config.clearBeforeUnlockSec) {
+          state.driveBlocked = false;
+          state.requiresNeutral = false;
+          state.stopReason = null;
+        }
+      }
+
+      const driveDirect = scaleDrive(state);
+      state.lastDriveOutput = { ...driveDirect };
+      state.lastDriveOutputAt = Date.now();
+      return { ...payload, driveDirect };
+    }
+
+    if (type === 'motors' && payload?.motorPwm) {
+      state.auxIntent = {
+        main: finiteNumber(payload.motorPwm.main),
+        side: finiteNumber(payload.motorPwm.side),
+        vacuum: finiteNumber(payload.motorPwm.vacuum),
+      };
+      const motorPwm = scaleAux(state);
+      state.lastAuxOutput = { ...motorPwm };
+      state.lastAuxOutputAt = Date.now();
+      return { ...payload, motorPwm };
+    }
+
+    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;
+
+    motor.overcurrent = Boolean(overcurrent);
+    motor.commandedSpeed = finiteNumber(command);
+    motor.measuredSpeed = measuredMagnitude;
+    motor.currentMa = Number.isFinite(Number(currentMa)) ? Number(currentMa) : null;
+    motor.stallFactor = stallFactor;
+    motor.stress = clampUnit(
+      motor.stress
+        + (motor.overcurrent ? riseRate * deltaSec : -config.wheelRecoveryRatePerSec * deltaSec),
+    );
+    motor.cap = calculateCap(motor.stress);
+  }
+
+  function updateBrushMotor(motor, { overcurrent, command, currentMa, deltaSec }) {
+    motor.overcurrent = Boolean(overcurrent);
+    motor.commandedSpeed = finiteNumber(command);
+    motor.measuredSpeed = null;
+    motor.currentMa = Number.isFinite(Number(currentMa)) ? Number(currentMa) : null;
+    motor.stallFactor = 0;
+    motor.stress = clampUnit(
+      motor.stress
+        + (motor.overcurrent
+          ? config.brushOvercurrentRatePerSec * deltaSec
+          : -config.brushRecoveryRatePerSec * deltaSec),
+    );
+    motor.cap = calculateCap(motor.stress);
+  }
+
+  function maybeStopDrive(roverId, state) {
+    if (state.bypassed || state.driveBlocked || !hasDriveIntent(state)) return;
+    const stalledWheel = ['leftWheel', 'rightWheel'].find((key) => state.motors[key].stress >= 1);
+    if (!stalledWheel) return;
+
+    state.driveBlocked = true;
+    state.requiresNeutral = true;
+    state.neutralSeen = false;
+    state.driveClearSec = 0;
+    state.stopReason = stalledWheel;
+    state.lastDriveOutputAt = Date.now();
+    state.lastDriveOutput = { left: 0, right: 0 };
+    issueAdjustedCommand(roverId, {
+      type: 'drive',
+      driveDirect: { left: 0, right: 0 },
+    });
+    logger.warn('Stopped rover after persistent wheel overcurrent', {
+      roverId,
+      motor: stalledWheel,
+    });
+  }
+
+  function maybeResendScaledOutputs(roverId, state, now) {
+    if (state.bypassed) return;
+
+    /*
+      A held keyboard/gamepad value may not emit another browser command while
+      telemetry continues changing the cap. Rate-limited resends make each new
+      server calculation effective without requiring the user to move the
+      control again or flooding the Pi websocket at sensor-frame cadence.
+    */
+    const nextDriveOutput = scaleDrive(state);
+    if (
+      hasDriveIntent(state)
+      && !state.driveBlocked
+      && !driveOutputsEqual(nextDriveOutput, state.lastDriveOutput)
+      && now - state.lastDriveOutputAt >= config.outputRateMs
+    ) {
+      const issued = issueAdjustedCommand(roverId, {
+        type: 'drive',
+        driveDirect: nextDriveOutput,
+      });
+      if (issued) {
+        state.lastDriveOutputAt = now;
+        state.lastDriveOutput = { ...nextDriveOutput };
+      }
+    }
+
+    const nextAuxOutput = scaleAux(state);
+    if (
+      hasAuxIntent(state)
+      && !auxOutputsEqual(nextAuxOutput, state.lastAuxOutput)
+      && now - state.lastAuxOutputAt >= config.outputRateMs
+    ) {
+      const issued = issueAdjustedCommand(roverId, {
+        type: 'motors',
+        motorPwm: nextAuxOutput,
+      });
+      if (issued) {
+        state.lastAuxOutputAt = now;
+        state.lastAuxOutput = { ...nextAuxOutput };
+      }
+    }
+  }
+
+  function processTelemetry(roverId, sensors = {}, now = Date.now()) {
+    const state = getState(roverId);
+    const timestamp = finiteNumber(now, Date.now());
+    const previousAt = state.lastTelemetryAt;
+    state.lastTelemetryAt = timestamp;
+
+    if (state.bypassed) {
+      /*
+        Admin bypass means telemetry remains visible but cannot build hidden
+        stress or schedule a delayed stop. Motor observations are still copied
+        into the public snapshot so administrators can see the hardware warning
+        while deliberately retaining full control.
+      */
+      state.driveBlocked = false;
+      state.requiresNeutral = false;
+      state.neutralSeen = false;
+      state.driveClearSec = 0;
+      state.stopReason = null;
+    }
+
+    const rawDeltaSec = previousAt > 0 ? Math.max(0, (timestamp - previousAt) / 1000) : 0;
+    const deltaSec = state.bypassed
+      ? 0
+      : Math.min(config.maxTelemetryDeltaSec, rawDeltaSec);
+    const flags = sensors?.wheelOvercurrents || {};
+    const speeds = sensors?.wheelSpeedsMmPerSecond || {};
+
+    updateWheelMotor(state.motors.leftWheel, {
+      overcurrent: flags.leftWheel,
+      command: state.driveIntent.left,
+      measured: speeds.left,
+      currentMa: sensors?.wheelLeftCurrentMa,
+      deltaSec,
+    });
+    updateWheelMotor(state.motors.rightWheel, {
+      overcurrent: flags.rightWheel,
+      command: state.driveIntent.right,
+      measured: speeds.right,
+      currentMa: sensors?.wheelRightCurrentMa,
+      deltaSec,
+    });
+    updateBrushMotor(state.motors.mainBrush, {
+      overcurrent: flags.mainBrush,
+      command: state.auxIntent.main,
+      currentMa: sensors?.mainBrushCurrentMa,
+      deltaSec,
+    });
+    updateBrushMotor(state.motors.sideBrush, {
+      overcurrent: flags.sideBrush,
+      command: state.auxIntent.side,
+      currentMa: sensors?.sideBrushCurrentMa,
+      deltaSec,
+    });
+
+    const wheelOvercurrent = Boolean(flags.leftWheel || flags.rightWheel);
+    state.driveClearSec = wheelOvercurrent ? 0 : state.driveClearSec + deltaSec;
+    if (
+      state.driveBlocked
+      && state.neutralSeen
+      && state.driveClearSec >= config.clearBeforeUnlockSec
+    ) {
+      state.driveBlocked = false;
+      state.requiresNeutral = false;
+      state.stopReason = null;
+    }
+
+    maybeStopDrive(roverId, state);
+    maybeResendScaledOutputs(roverId, state, timestamp);
+    return getPublicState(roverId);
+  }
+
+  function getStatus(state) {
+    const anyOvercurrent = MOTOR_KEYS.some((key) => state.motors[key].overcurrent);
+    if (state.bypassed && anyOvercurrent) return 'bypassed';
+    if (state.driveBlocked) return 'stopped';
+    const anyLimited = MOTOR_KEYS.some((key) => state.motors[key].cap < 1);
+    if (anyLimited && anyOvercurrent) return 'limiting';
+    if (anyLimited) return 'recovering';
+    return 'idle';
+  }
+
+  function getPublicState(roverId) {
+    const state = getState(roverId);
+    const motors = MOTOR_KEYS.reduce((result, key) => {
+      result[key] = { ...state.motors[key] };
+      return result;
+    }, {});
+    return {
+      status: getStatus(state),
+      bypassed: state.bypassed,
+      drive: {
+        cap: getDriveCap(state),
+        blocked: state.driveBlocked,
+        requiresNeutral: state.requiresNeutral,
+        clearSec: state.driveClearSec,
+        stopReason: state.stopReason,
+      },
+      motors,
+      config: { ...config },
+    };
+  }
+
+  function cleanupRover(roverId) {
+    states.delete(String(roverId || ''));
+  }
+
+  return {
+    configureCommandIssuer,
+    protectCommand,
+    processTelemetry,
+    getPublicState,
+    cleanupRover,
+  };
+}
+
+const service = createOvercurrentProtectionService();
+
+module.exports = {
+  ...service,
+  createOvercurrentProtectionService,
+  DEFAULT_CONFIG,
+};

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

@@ -0,0 +1,170 @@
+// Overcurrent Protection Service Tests
+// Purpose: Verifies stress integration, administrator bypass, neutral recovery, and independent brush limiting.
+// Scope: Exercises the service as a pure state machine with an injected command sink; no rover or socket process is started.
+
+const assert = require('node:assert/strict');
+const test = require('node:test');
+const { createOvercurrentProtectionService } = require('./index');
+
+function makeSensors(overrides = {}) {
+  return {
+    wheelOvercurrents: {
+      leftWheel: false,
+      rightWheel: false,
+      mainBrush: false,
+      sideBrush: false,
+      ...(overrides.wheelOvercurrents || {}),
+    },
+    wheelSpeedsMmPerSecond: {
+      left: 300,
+      right: 300,
+      ...(overrides.wheelSpeedsMmPerSecond || {}),
+    },
+  };
+}
+
+function createHarness(config = {}) {
+  const issued = [];
+  const service = createOvercurrentProtectionService({
+    config,
+    issueCommand: (roverId, payload) => issued.push({ roverId, payload }),
+  });
+  return { service, issued };
+}
+
+test('a short stalled-wheel spike remains inside the grace region', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+
+  service.processTelemetry('rover', makeSensors({
+    wheelOvercurrents: { leftWheel: true },
+    wheelSpeedsMmPerSecond: { left: 0 },
+  }), start);
+  const snapshot = service.processTelemetry('rover', makeSensors({
+    wheelOvercurrents: { leftWheel: true },
+    wheelSpeedsMmPerSecond: { left: 0 },
+  }), start + 100);
+
+  assert.equal(snapshot.motors.leftWheel.stress, 0.2);
+  assert.equal(snapshot.motors.leftWheel.cap, 1);
+  assert.equal(snapshot.status, 'idle');
+});
+
+test('persistent stalled-wheel overcurrent scales both wheels and then stops drive', () => {
+  const { service, issued } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 200 },
+  });
+
+  for (let step = 0; step <= 5; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: 0, right: 200 },
+    }), start + step * 100);
+  }
+
+  const snapshot = service.getPublicState('rover');
+  assert.equal(snapshot.status, 'stopped');
+  assert.equal(snapshot.drive.blocked, true);
+  assert.equal(snapshot.drive.requiresNeutral, true);
+  assert.equal(snapshot.drive.stopReason, 'leftWheel');
+  assert.deepEqual(issued.at(-1), {
+    roverId: 'rover',
+    payload: { type: 'drive', driveDirect: { left: 0, right: 0 } },
+  });
+
+  /*
+    Before the hard stop, any rate-limited drive update must use one shared cap.
+    This preserves the requested curve instead of driving the healthy wheel at
+    full output around the mechanically obstructed side.
+  */
+  const scaledDrive = issued.find((entry) => entry.payload.type === 'drive'
+    && entry.payload.driveDirect.left > 0);
+  assert.ok(scaledDrive);
+  assert.equal(
+    scaledDrive.payload.driveDirect.left / 300,
+    scaledDrive.payload.driveDirect.right / 200,
+  );
+});
+
+test('administrator commands and telemetry bypass all enforcement', () => {
+  const { service, issued } = createHarness({ outputRateMs: 0 });
+  const start = Date.now();
+  const command = service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 500, right: -500 },
+  }, { bypassed: true });
+
+  for (let step = 0; step <= 20; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true, rightWheel: true },
+      wheelSpeedsMmPerSecond: { left: 0, right: 0 },
+    }), start + step * 100);
+  }
+
+  const snapshot = service.getPublicState('rover');
+  assert.deepEqual(command.driveDirect, { left: 500, right: -500 });
+  assert.equal(snapshot.status, 'bypassed');
+  assert.equal(snapshot.bypassed, true);
+  assert.equal(snapshot.motors.leftWheel.stress, 0);
+  assert.equal(snapshot.motors.rightWheel.stress, 0);
+  assert.equal(snapshot.drive.blocked, false);
+  assert.equal(issued.length, 0);
+});
+
+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 <= 5; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { leftWheel: true },
+      wheelSpeedsMmPerSecond: { left: 0 },
+    }), start + step * 100);
+  }
+
+  for (let step = 6; step <= 14; step += 1) {
+    service.processTelemetry('rover', makeSensors(), start + step * 100);
+  }
+  assert.equal(service.getPublicState('rover').drive.blocked, true);
+
+  const heldCommand = service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+  assert.deepEqual(heldCommand.driveDirect, { left: 0, right: 0 });
+
+  service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 0, right: 0 },
+  });
+  assert.equal(service.getPublicState('rover').drive.blocked, false);
+
+  const resumed = service.protectCommand('rover', 'drive', {
+    driveDirect: { left: 300, right: 300 },
+  });
+  assert.deepEqual(resumed.driveDirect, { left: 300, right: 300 });
+});
+
+test('brush stress limits only the brush that reports overcurrent', () => {
+  const { service } = createHarness();
+  const start = Date.now();
+  service.protectCommand('rover', 'motors', {
+    motorPwm: { main: 100, side: 100, vacuum: 100 },
+  });
+  for (let step = 0; step <= 4; step += 1) {
+    service.processTelemetry('rover', makeSensors({
+      wheelOvercurrents: { mainBrush: true },
+    }), start + step * 100);
+  }
+
+  const protectedCommand = service.protectCommand('rover', 'motors', {
+    motorPwm: { main: 100, side: 100, vacuum: 100 },
+  });
+  assert.ok(protectedCommand.motorPwm.main < 100);
+  assert.equal(protectedCommand.motorPwm.side, 100);
+  assert.equal(protectedCommand.motorPwm.vacuum, 100);
+});