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:
co-authored by
Claude Opus 4.8
parent
6374efc26a
commit
7615ecd316
@@ -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
|
||||||
|
|||||||
Reference in New Issue
Block a user