MP: FIX peer spin freeze/half-rate -- unparenthesized Abs() macro floods angular resync (task #50)

Root cause found via autonomous headless spin (BT_AUTODRIVE+BT_FORCE_TURN) +
frame-level [angsign] probe: the STYLE.H:118 macro

    #define Abs(value) ((value>0) ? value : -value)

has NO parens around value, so Abs(a-b) mis-expands to (a-b>0 ? a-b : -a-b) ==
-(a+b) on the false branch, NOT |a-b|.  The angular resync velocity gate passed
an EXPRESSION: Abs(localVelocity.angular.y - updateVelocity.angular.y).  For a
steady spin the two are equal, so a-b==0 takes the false branch and yields
-(2*rate): on a REVERSE spin that is +2*rate > velDb, firing a type-4 resync
EVERY frame.  The resync flood reset the peer's dead-reckon horizon every frame,
pinning its angular slerp at ~half rate and (as the drift periodically ran to
180deg) freezing the rotation for seconds while the leg turn-clip kept stomping.

Confirmed live-autonomous: velDrift 0 (was +2.618), byVel 0 (was 55/55), peer
rendered-rotation median dyaw/expected 1.00 (was 0.49), no multi-second dt gaps.

Fix (mech4.cpp resync gate, reconstruction side -- the engine macro is original,
the original master-perf avoided it by diffing into a temp):
 - velDrift: diff into a temp, explicit (d<0?-d:d) so the macro never sees an
   expression.
 - angDrift: already reworked to a single-variable wrap-safe yaw delta (also
   dodges the macro) -- and it corrects a prior units/wrap bug (was comparing a
   raw quaternion-Y component against a radian deadband).
 - velocity gate now diffs vs updateVelocity (the value the peer extrapolates
   with), same scalar representation as localVelocity.
 - BT_SPIN / BT_ANGSIGN / updYaw diagnostic probes (env-gated).

KNOWN RESIDUAL (smaller, follow-up): the ANGLE gate still bursts when the crude
large-angle quaternion projection in the reckoner runs updateOrigin stale between
writes and drifts ~180deg; causes occasional single-frame hitches (peer median is
still 1.0), not the freeze.  Separate from this fix.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
arcattack
2026-07-14 16:04:32 -05:00
co-authored by Claude Opus 4.8
parent 6374efc26a
commit 7615ecd316
+74 -8
View File
@@ -3591,17 +3591,62 @@ void
Scalar angDb = (updateTurnAngleDeadband > 0.0f) ? updateTurnAngleDeadband : -1.0f; Scalar angDb = (updateTurnAngleDeadband > 0.0f) ? updateTurnAngleDeadband : -1.0f;
Scalar velDb = (updateTurnVelocityDeadband > 0.0f) ? updateTurnVelocityDeadband : -1.0f; Scalar velDb = (updateTurnVelocityDeadband > 0.0f) ? updateTurnVelocityDeadband : -1.0f;
Logical resync = False; Logical resync = False;
int rzn = 0; // 1=angleDrift 2=velDrift 4=cameToRestAng
// TRUE yaw drift in RADIANS (wrap-safe), to match the radian deadband
// angDb. The prior `Abs(localOrigin.angularPosition.y -
// projectedOrigin.angularPosition.y)` compared raw QUATERNION y-components
// (sin(yaw/2)-ish, unitless) against a radian deadband -- both a units
// mismatch and wrap-unsafe: at the +-pi heading wrap the quaternion-y diff
// spikes toward its max (~2) and fired byAngle=56/56 EVERY frame (measured),
// flooding resyncs at every wrap. Take the yaw difference in euler space
// and unwrap it to [-pi,pi] so a wrap is not seen as a ~2pi drift.
YawPitchRoll _yprL, _yprP;
_yprL = localOrigin.angularPosition;
_yprP = projectedOrigin.angularPosition;
Scalar _dyaw = (Scalar)(_yprL.yaw - _yprP.yaw);
while (_dyaw > 3.14159265f) _dyaw -= 6.28318531f;
while (_dyaw < -3.14159265f) _dyaw += 6.28318531f;
const Scalar angDrift = Abs(_dyaw);
// Compare the live yaw rate against the LAST-SENT rate (updateVelocity,
// which the type-4 writer copies straight from localVelocity, mech.cpp:2107)
// -- NOT projectedVelocity, which the master's dead-reckoner recomputes in a
// different representation/sign so the scalar-.y compare always read the full
// 2x mismatch and FLOODED a resync every frame (measured: byVel=55/55,
// velDrift=2.618=2x the 1.309 spin rate). The flood reset the peer's
// dead-reckon horizon every frame, pinning its slerp at ~half rate -> the
// "rotates slow then jumps" spin. updateVelocity is the value the peer
// actually extrapolates with, in the same units, so a steady spin now drifts
// 0 (no resync) and the peer extrapolates smoothly; a real rate change still
// diverges past velDb and fires.
// NB: the Abs() macro (STYLE.H:118) is UNPARENTHESIZED --
// Abs(a-b) mis-expands to (a-b>0 ? a-b : -a-b) == -(a+b) on the false
// branch, NOT |a-b|. For a steady spin (localVy==updVy) that yields
// -(2*rate): on a reverse spin it is +2*rate and FLOODED a resync every
// frame (the confirmed root of the half-rate/freeze). Diff into a temp and
// take an explicit abs so the macro only ever sees a single token.
const Scalar velDiff = (Scalar)localVelocity.angularMotion.y - (Scalar)updateVelocity.angularMotion.y;
const Scalar velDrift = (velDiff < 0.0f) ? -velDiff : velDiff;
// ANGULAR SIGN hunt (BT_ANGSIGN, 0.2s): instantaneous gate values, to
// catch which of local/update/projected angular-Y carries the wrong sign.
if (getenv("BT_ANGSIGN"))
{
static float s_as = 0.0f; s_as += dt;
if (s_as >= 0.2f)
{
s_as = 0.0f;
DEBUG_STREAM << "[angsign] localVy=" << (Scalar)localVelocity.angularMotion.y
<< " updVy=" << (Scalar)updateVelocity.angularMotion.y
<< " projVy=" << (Scalar)projectedVelocity.angularMotion.y
<< " velDrift=" << velDrift << " angDrift=" << angDrift
<< " turn=" << turn << "\n" << std::flush;
}
}
if (angDb > 0.0f) if (angDb > 0.0f)
{ {
if (Abs(localOrigin.angularPosition.y if (angDrift > angDb) { resync = True; rzn |= 1; }
- projectedOrigin.angularPosition.y) > angDb) if (velDrift > velDb) { resync = True; rzn |= 2; }
resync = True;
if (Abs((Scalar)localVelocity.angularMotion.y
- (Scalar)projectedVelocity.angularMotion.y) > velDb)
resync = True;
if ((Scalar)localVelocity.angularMotion.y == 0.0f if ((Scalar)localVelocity.angularMotion.y == 0.0f
&& (Scalar)updateVelocity.angularMotion.y != 0.0f) && (Scalar)updateVelocity.angularMotion.y != 0.0f) { resync = True; rzn |= 4; }
resync = True;
} }
else if (Abs(angular_deviation.w) < 0.997f) else if (Abs(angular_deviation.w) < 0.997f)
{ {
@@ -3611,6 +3656,27 @@ void
{ {
ForceUpdate(1 << MechResyncUpdateModelBit); // type 4 ForceUpdate(1 << MechResyncUpdateModelBit); // type 4
} }
// SPIN diagnosis (BT_SPIN, once/sec): how DENSE are the orientation
// (type-4) records while turning? A stuttery peer spin == too sparse.
if (getenv("BT_SPIN"))
{
static float sAcc = 0.0f; static int sResync = 0, sFrames = 0, sAng = 0, sVel = 0, sRest = 0;
static float sMaxAng = 0.0f, sMaxVel = 0.0f;
sAcc += dt; sFrames++; if (resync) sResync++;
if (rzn & 1) sAng++; if (rzn & 2) sVel++; if (rzn & 4) sRest++;
if (angDrift > sMaxAng) sMaxAng = angDrift;
if (velDrift > sMaxVel) sMaxVel = velDrift;
if (sAcc >= 1.0f)
{
DEBUG_STREAM << "[spin-tx] resyncs=" << sResync << "/" << sFrames
<< " byAngle=" << sAng << " byVel=" << sVel << " byRest=" << sRest
<< " maxAng=" << sMaxAng << "(db" << angDb << ")"
<< " maxVel=" << sMaxVel << "(db" << velDb << ")"
<< " liveYaw=" << (Scalar)localVelocity.angularMotion.y
<< " updYaw=" << (Scalar)updateVelocity.angularMotion.y << "\n" << std::flush;
sAcc = 0.0f; sResync = 0; sFrames = 0; sAng = 0; sVel = 0; sRest = 0; sMaxAng = 0.0f; sMaxVel = 0.0f;
}
}
} }
// Commanded-speed deadband (binary @0x4aac88): the mapper's live // Commanded-speed deadband (binary @0x4aac88): the mapper's live
// speedDemand vs the last-replicated bodyTargetSpeed -> the tiny // speedDemand vs the last-replicated bodyTargetSpeed -> the tiny