diff --git a/game/reconstructed/mech4.cpp b/game/reconstructed/mech4.cpp index a56b001..d0c87ca 100644 --- a/game/reconstructed/mech4.cpp +++ b/game/reconstructed/mech4.cpp @@ -2095,6 +2095,32 @@ void legCycleSpeed = 0.0f; (void)AdvanceLegAnimation(dt); // joints only; travel = DeadReckon + // TURN-STEP rate probe (BT_TRNRATE, 0.5s): body rotation rate vs trn clip advance. + if (getenv("BT_TRNRATE")) + { + static bool s_tinit = false; + static float s_tYaw = 0.0f, s_tFrm = 0.0f, s_tAcc = 0.0f, s_tRot = 0.0f, s_tAdv = 0.0f; + YawPitchRoll _typ; _typ = localOrigin.angularPosition; + const float yaw = (float)_typ.yaw, frm = (float)legAnimation.currentFrame; + if (s_tinit) + { + float dy = yaw - s_tYaw; if (dy > 3.14159f) dy -= 6.28319f; if (dy < -3.14159f) dy += 6.28319f; + s_tRot += (dy < 0.0f) ? -dy : dy; + const float df = frm - s_tFrm; + if (df > 0.0f) s_tAdv += df; + } + s_tYaw = yaw; s_tFrm = frm; s_tinit = true; + s_tAcc += dt; + if (s_tAcc >= 0.5f) + { + DEBUG_STREAM << "[trnrate] bodyRot/s=" << (s_tRot / s_tAcc) << "rad" + << " legFrm/s=" << (s_tAdv / s_tAcc) + << " legState=" << (int)legStateAlarm.GetLevel() + << " turnDemand=" << MappingMapper()->turnDemand << "\n" << std::flush; + s_tAcc = 0.0f; s_tRot = 0.0f; s_tAdv = 0.0f; + } + } + // Cadence-vs-travel sync probe (0.25s, ~4 flush/s -- cheap). Compares // the leg CADENCE input (bodyTargetSpeed / legCycleSpeed) against the // ACTUAL dead-reckoned ground speed; if the legs are in sync the two