Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp.Client/Components/Motion/OdometryCard.razor

@@ -0,0 +1,121 @@
+@using RobotNet10.RobotApp.Client.Shared.Motion
+@using RobotNet10.Shared.Geometry
+@using RobotNet10.Shared.Numbers
+@using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion
+@using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion
+@using MudBlazor
+@implements IDisposable
+
+<MudPaper Class="pa-4" Style="height: 100%;">
+    <MudStack Spacing="3">
+        <MudStack Row="true" Justify="Justify.SpaceBetween" AlignItems="AlignItems.Center">
+            <MudText Typo="Typo.h5">Odometry</MudText>
+        </MudStack>
+
+        @* Odometry Information *@
+        @if (Odometry != null)
+        {
+            <MudCard>
+                <MudCardContent>
+                    <MudText Typo="Typo.h6" Class="mb-3">Position</MudText>
+                    <MudStack Spacing="2">
+                        <MudStack Row="true" Justify="Justify.SpaceBetween">
+                            <MudText>X:</MudText>
+                            <MudText>@($"{Odometry.PositionX:F3} m")</MudText>
+                        </MudStack>
+                        <MudStack Row="true" Justify="Justify.SpaceBetween">
+                            <MudText>Y:</MudText>
+                            <MudText>@($"{Odometry.PositionY:F3} m")</MudText>
+                        </MudStack>
+                        <MudStack Row="true" Justify="Justify.SpaceBetween">
+                            <MudText>Z:</MudText>
+                            <MudText>@($"{Odometry.PositionZ:F3} m")</MudText>
+                        </MudStack>
+                    </MudStack>
+                </MudCardContent>
+            </MudCard>
+
+            <MudCard>
+                <MudCardContent>
+                    <MudText Typo="Typo.h6" Class="mb-3">Orientation</MudText>
+                    <MudStack Spacing="2">
+                        <MudStack Row="true" Justify="Justify.SpaceBetween">
+                            <MudText>Yaw:</MudText>
+                            <MudText>@($"{yawDegrees:F2}°")</MudText>
+                        </MudStack>
+                        <MudStack Row="true" Justify="Justify.SpaceBetween">
+                            <MudText>Update Freq:</MudText>
+                            <MudChip T="double" Color="@(Odometry.UpdateFrequency > 0 ? Color.Success : Color.Default)" Size="Size.Small">
+                                @($"{Odometry.UpdateFrequency:F2} Hz")
+                            </MudChip>
+                        </MudStack>
+                    </MudStack>
+                </MudCardContent>
+            </MudCard>
+        }
+        else
+        {
+            <MudAlert Severity="Severity.Info">No odometry data available.</MudAlert>
+        }
+    </MudStack>
+</MudPaper>
+
+@code {
+    [Parameter, EditorRequired] public OdometryDto? Odometry { get; set; }
+    [Parameter] public bool IsHubReady { get; set; }
+    [Inject] private ISnackbar Snackbar { get; set; } = null!;
+
+    private double _smoothedYawDegrees;
+    private bool _hasYaw;
+
+    private double yawDegrees => _smoothedYawDegrees;
+
+    protected override void OnParametersSet()
+    {
+        if (Odometry == null)
+            return;
+
+        // Nếu robot gần như đứng yên (ít quay, ít chạy thẳng) thì giữ nguyên yaw
+        var angularZ = Odometry.AngularVelocityZ;   // rad/s
+        var linearX = Odometry.LinearVelocityX;     // m/s
+        const double angularDeadZone = 0.01;        // ~0.57°
+        const double linearDeadZone = 0.005;        // 5 mm/s
+
+        var isAlmostStopped =
+            Math.Abs(angularZ) < angularDeadZone &&
+            Math.Abs(linearX) < linearDeadZone;
+
+        // Lần đầu vẫn phải init giá trị
+        if (!_hasYaw)
+        {
+            var q0 = new QuaternionGeometry(Odometry.OrientationX, Odometry.OrientationY, Odometry.OrientationZ, Odometry.OrientationW);
+            _smoothedYawDegrees = q0.ToYawDegrees();
+            _hasYaw = true;
+            return;
+        }
+
+        if (isAlmostStopped)
+        {
+            // Robot đứng yên: không cập nhật yaw để tránh drift chậm
+            return;
+        }
+
+        // Robot đang quay / di chuyển: cho phép yaw thay đổi nhưng có lọc
+        var quaternion = new QuaternionGeometry(Odometry.OrientationX, Odometry.OrientationY, Odometry.OrientationZ, Odometry.OrientationW);
+        var newYaw = quaternion.ToYawDegrees();
+
+        // Normalize delta để tránh nhảy 360° khi wrap-around
+        var delta = newYaw - _smoothedYawDegrees;
+        while (delta > 180.0) delta -= 360.0;
+        while (delta < -180.0) delta += 360.0;
+
+        // Low-pass filter để làm mượt (alpha càng nhỏ càng mượt)
+        const double alpha = 0.3;
+        _smoothedYawDegrees = _smoothedYawDegrees + alpha * delta;
+    }
+
+    public void Dispose()
+    {
+        // Nothing to dispose currently
+    }
+}