Add fleet correction diagnostics

2026-07-01 23:10:13 +08:00
parent 1c0258ec75
commit f127e145e1
1 changed files with 4 additions and 0 deletions
@@ -1284,8 +1284,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
                $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} outBias({Conf.TwoLegOutputBiasX:F0},{Conf.TwoLegOutputBiasY:F0}) " +
                $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
+                $"fac({Conf.MultiVehicleDetectBiasXFac:F2},{Conf.MultiVehicleDetectBiasYFac:F2},{Conf.MultiVehicleDetectBiasThFac:F2}) " +
+                $"lim({Conf.MultiVehicleDetectBiasXThreshold:F1},{Conf.MultiVehicleDetectBiasYThreshold:F1},{Conf.MultiVehicleDetectBiasThThreshold:F1}) " +
                $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
                $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
+                $"fac({Conf.MultiVehiclePosBiasXFac:F2},{Conf.MultiVehiclePosBiasYFac:F2},{Conf.MultiVehiclePosBiasThFac:F2}) " +
+                $"lim({Conf.MultiVehiclePosBiasXThreshold:F1},{Conf.MultiVehiclePosBiasYThreshold:F1},{Conf.MultiVehiclePosBiasThThreshold:F1}) " +
                $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
                $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +