122 lines
4.8 KiB
Plaintext
122 lines
4.8 KiB
Plaintext
@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
|
|
}
|
|
}
|