mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 09:31:20 -04:00
slopcurrent
This commit is contained in:
@@ -9,6 +9,7 @@ const { isDeterred } = require('../verificationService');
|
||||
const logger = require('../../globals/logger').child('commandService');
|
||||
const { isHeadlightBlocked } = require('../../rewards/definitions/darkness');
|
||||
const homeAssistantService = require('../homeAssistantService');
|
||||
const overcurrentProtectionService = require('../overcurrentProtectionService');
|
||||
|
||||
const pendingCommands = new Map(); // id -> { roverId }
|
||||
const lastDriveActivity = new Map(); // roverId -> { ts, socketId, direction, speed, isAdmin }
|
||||
@@ -93,6 +94,31 @@ function issueCommand(roverId, payload) {
|
||||
return id;
|
||||
}
|
||||
|
||||
/*
|
||||
The protection service owns decisions about when a held command must be
|
||||
resent at a lower output. Injecting this raw transport function keeps those
|
||||
resends on the same rover websocket path as every other server command while
|
||||
avoiding a circular dependency from the protection service back into this
|
||||
socket-facing module.
|
||||
*/
|
||||
overcurrentProtectionService.configureCommandIssuer((roverId, payload) => {
|
||||
const blockedUntil = driveCooldowns.get(roverId);
|
||||
const safetyCooldownActive = blockedUntil && Date.now() < blockedUntil;
|
||||
if (safetyCooldownActive && getCommandMotionMagnitude(payload?.type, payload) > 0) {
|
||||
/*
|
||||
Private-rover and dock safety own the existing command cooldown map. A
|
||||
rate-limited protection resend must respect those independent systems;
|
||||
otherwise this new service could restart drive or brushes immediately
|
||||
after an unrelated safety feature deliberately stopped them. Returning
|
||||
false tells the protection service to retry after the cooldown instead of
|
||||
recording an output that never reached the rover.
|
||||
*/
|
||||
return false;
|
||||
}
|
||||
issueCommand(roverId, payload);
|
||||
return true;
|
||||
});
|
||||
|
||||
function handleAck(msg) {
|
||||
const pending = pendingCommands.get(msg.id);
|
||||
if (!pending) return;
|
||||
@@ -226,7 +252,7 @@ io.on('connection', (socket) => {
|
||||
if (type === 'audioLevels') {
|
||||
throw new Error('audioLevels command is service-managed');
|
||||
}
|
||||
const payload = data ? { ...data } : {};
|
||||
let payload = data ? { ...data } : {};
|
||||
if (type === 'headlight' && isHeadlightBlocked()) {
|
||||
logger.info('Ignoring headlight command while darkness lock is active', { socketId: socket.id, roverId });
|
||||
reply({ ignored: true, reason: 'darknessActive' });
|
||||
@@ -291,6 +317,19 @@ io.on('connection', (socket) => {
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if (type === 'drive' || type === 'motors') {
|
||||
/*
|
||||
Role is supplied at the command boundary because telemetry does not
|
||||
identify the operator who produced the active motor intent. Admin and
|
||||
lockdown commands therefore enter the service explicitly bypassed;
|
||||
they are recorded for status visibility but are never scaled, blocked,
|
||||
or countermanded by a later sensor frame.
|
||||
*/
|
||||
payload = overcurrentProtectionService.protectCommand(roverId, type, payload, {
|
||||
bypassed: isAdminSocket,
|
||||
});
|
||||
}
|
||||
const id = issueCommand(roverId, { type, ...payload });
|
||||
logger.info('Queued command', socket.id, roverId, type);
|
||||
if (shouldRecordTurnActivity(type, payload)) {
|
||||
|
||||
@@ -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,
|
||||
};
|
||||
@@ -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);
|
||||
});
|
||||
@@ -6,6 +6,7 @@ const logger = require('../../globals/logger').child('roverManager');
|
||||
const { sendAlert } = require('../alertService');
|
||||
const { parseSensorFrame } = require('../../helpers/sensorDecoder');
|
||||
const odometerService = require('../odometerService');
|
||||
const overcurrentProtectionService = require('../overcurrentProtectionService');
|
||||
const { MODES, getMode } = require('../modeManager');
|
||||
const { isAdmin, isLockdownAdmin, roleEvents } = require('../roleService');
|
||||
const { publishEvent } = require('../eventBus');
|
||||
@@ -179,6 +180,7 @@ const sensorPipeline = createSensorPipeline({
|
||||
sendAlert,
|
||||
publishEvent,
|
||||
processOdometerFrame: odometerService.processSensorFrame,
|
||||
processOvercurrentTelemetry: overcurrentProtectionService.processTelemetry,
|
||||
isPrivateRecord,
|
||||
isPrivateOpen,
|
||||
getPrivateSafety,
|
||||
@@ -190,6 +192,18 @@ const sensorPipeline = createSensorPipeline({
|
||||
const { handleSensorFrame, applyPrivateDriveSafety } = sensorPipeline;
|
||||
stopDockGuard = sensorPipeline.stopDockGuard;
|
||||
|
||||
managerEvents.on('rover', ({ roverId, action }) => {
|
||||
/*
|
||||
Protection state contains the last motor intent for a specific physical
|
||||
rover connection. Removing it with the roster record prevents a reconnect
|
||||
from inheriting stale stress, an old administrator bypass, or a neutral
|
||||
requirement from the previous connection.
|
||||
*/
|
||||
if (action === 'removed') {
|
||||
overcurrentProtectionService.cleanupRover(roverId);
|
||||
}
|
||||
});
|
||||
|
||||
function removeSocket(socket) {
|
||||
roverLifecycle.removeSocket(socket, disableSpectator);
|
||||
}
|
||||
|
||||
@@ -31,6 +31,7 @@ function createSensorPipeline(deps) {
|
||||
sendAlert,
|
||||
publishEvent,
|
||||
processOdometerFrame,
|
||||
processOvercurrentTelemetry,
|
||||
isPrivateRecord,
|
||||
isPrivateOpen,
|
||||
getPrivateSafety,
|
||||
@@ -551,6 +552,19 @@ function createSensorPipeline(deps) {
|
||||
};
|
||||
record.lastSensor = { raw: frame, decoded };
|
||||
}
|
||||
/*
|
||||
Rover manager remains responsible only for decoding and routing sensor
|
||||
frames. The dedicated service receives the completed sensor object after
|
||||
odometry has added measured wheel speeds, because requested-versus-actual
|
||||
motion is the evidence that distinguishes a transient current spike from
|
||||
a mechanically stalled wheel.
|
||||
*/
|
||||
// Use server arrival time inside the service rather than the Pi timestamp.
|
||||
// Raspberry Pi clocks can differ across the fleet, while command resend
|
||||
// throttling is also measured on this server and needs one clock domain.
|
||||
const overcurrentProtection = decoded && typeof processOvercurrentTelemetry === 'function'
|
||||
? processOvercurrentTelemetry(roverId, decoded)
|
||||
: null;
|
||||
updateMovement(record, decoded);
|
||||
const hasDockInfo = decoded?.chargingSources != null;
|
||||
if (hasDockInfo) {
|
||||
@@ -563,8 +577,18 @@ function createSensorPipeline(deps) {
|
||||
if (bumps?.bumpLeft || bumps?.bumpRight) record.lastBumpAt = Date.now();
|
||||
handlePrivateButtonHold(record, decoded);
|
||||
evaluatePrivateSafety(record, decoded);
|
||||
io.to(record.room).volatile.emit('sensorFrame', { roverId, frame, sensors: decoded });
|
||||
managerEvents.emit('sensor', { roverId, sensors: decoded, batteryState: record.batteryState });
|
||||
io.to(record.room).volatile.emit('sensorFrame', {
|
||||
roverId,
|
||||
frame,
|
||||
sensors: decoded,
|
||||
overcurrentProtection,
|
||||
});
|
||||
managerEvents.emit('sensor', {
|
||||
roverId,
|
||||
sensors: decoded,
|
||||
batteryState: record.batteryState,
|
||||
overcurrentProtection,
|
||||
});
|
||||
evaluateDockGuard(record, decoded);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user