mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-15 17:12:59 -04:00
slopcurrent
This commit is contained in:
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -78,8 +78,8 @@
|
||||
<script defer src="https://analytics.otter.land/script.js" data-website-id="82dd56a5-db44-4279-bd1e-a4d9fee39af7" data-domains="rover.otter.land"></script>
|
||||
<script defer src="https://analytics.otter.land/recorder.js" data-website-id="82dd56a5-db44-4279-bd1e-a4d9fee39af7" data-domains="rover.otter.land" data-sample-rate="0.15" data-mask-level="moderate" data-max-duration="300000"></script>
|
||||
<title>Roomba Rover</title>
|
||||
<script type="module" crossorigin src="/assets/index-Da9ufxPv.js"></script>
|
||||
<link rel="stylesheet" crossorigin href="/assets/index-BcBTKEa5.css">
|
||||
<script type="module" crossorigin src="/assets/index-DzMZA-qn.js"></script>
|
||||
<link rel="stylesheet" crossorigin href="/assets/index-DzqCFmyF.css">
|
||||
</head>
|
||||
<body>
|
||||
<div id="root"></div>
|
||||
|
||||
@@ -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