Initial commit
This commit is contained in:
@@ -0,0 +1,428 @@
|
||||
using System.Timers;
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
using RobotNet10.Shared;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Tada;
|
||||
|
||||
/// <summary>
|
||||
/// BMU driver cho TADA RS485
|
||||
/// </summary>
|
||||
[Device(DeviceType.Battery, "Tada", "TadaBattery", "1.0.0", Description = "Battery Tada Driver")]
|
||||
public class TadaBattery : DeviceBase, IBattery
|
||||
{
|
||||
private readonly ILogger<TadaBattery> _logger;
|
||||
private readonly Lock _dataLock = new();
|
||||
|
||||
private TadaRs485Client? _client;
|
||||
|
||||
// Polling
|
||||
private System.Timers.Timer? _pollingTimer;
|
||||
|
||||
// Cached (THEO TÀI LIỆU RS485)
|
||||
private double _cachedChargeLevel; // SOC %
|
||||
private double _cachedVoltage; // V
|
||||
private double _cachedCurrent; // A
|
||||
private bool _cachedCharging; // Current > 0
|
||||
|
||||
private double? _cachedHealth; // SOH %
|
||||
private double? _cachedTemperature; // Celsius
|
||||
private double? _cachedRemainingCapacity; // Ah
|
||||
private double? _cachedFullCapacity; // Wh (RemainEnergy)
|
||||
|
||||
private int? _cachedChargeTime; // minutes (time to full)
|
||||
private int? _cachedDischargeTime; // minutes (time to empty)
|
||||
private int? _cachedStatusRaw; // raw bit flags
|
||||
|
||||
private DateTime _lastUpdateTime = DateTime.MinValue;
|
||||
private BatteryState? _cachedBatteryState;
|
||||
|
||||
// IBattery Implementation
|
||||
public BatteryState? CurrentBatteryState
|
||||
{
|
||||
get { lock (_dataLock) { return _cachedBatteryState; } }
|
||||
}
|
||||
|
||||
private readonly string _portName;
|
||||
private readonly int _baud;
|
||||
|
||||
public TadaBattery(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
|
||||
: base(deviceId, deviceName, DeviceType.Battery)
|
||||
{
|
||||
|
||||
var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
|
||||
_logger = loggerFactory.CreateLogger<TadaBattery>();
|
||||
|
||||
_portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
|
||||
_baud = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
|
||||
|
||||
AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true;
|
||||
ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 2000;
|
||||
MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0;
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
// DeviceBase overrides
|
||||
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_client?.Dispose();
|
||||
_client = new TadaRs485Client(_logger, _portName, _baud);
|
||||
}
|
||||
UpdateProperties();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StartPollingLoop();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopPollingLoop();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopPollingLoop();
|
||||
lock (_dataLock)
|
||||
{
|
||||
_client?.ForceReconnect(); // now exists
|
||||
ResetCache();
|
||||
}
|
||||
UpdateProperties();
|
||||
StartPollingLoop();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
return Task.FromResult(_client != null && !_client.IsFaulted);
|
||||
}
|
||||
|
||||
// Polling
|
||||
private void StartPollingLoop()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_pollingTimer != null && _pollingTimer.Enabled)
|
||||
return;
|
||||
|
||||
StopPollingLoop(); // Đảm bảo không có timer nào đang chạy
|
||||
|
||||
_pollingTimer = new System.Timers.Timer(500) // 2Hz = 500ms interval
|
||||
{
|
||||
AutoReset = true,
|
||||
Enabled = true
|
||||
};
|
||||
_pollingTimer.Elapsed += PollTimer_Elapsed;
|
||||
}
|
||||
}
|
||||
|
||||
private void StopPollingLoop()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_pollingTimer != null)
|
||||
{
|
||||
_pollingTimer.Stop();
|
||||
_pollingTimer.Elapsed -= PollTimer_Elapsed;
|
||||
_pollingTimer.Dispose();
|
||||
_pollingTimer = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void PollTimer_Elapsed(object? sender, ElapsedEventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
_client?.RequestStatus();
|
||||
var data = _client?.ReadResponse();
|
||||
if (data != null)
|
||||
UpdateCache(data);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "RS485 polling error");
|
||||
}
|
||||
}
|
||||
|
||||
// UpdateCache
|
||||
private void UpdateCache(Dictionary<string, double> data)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
var now = DateTime.UtcNow;
|
||||
|
||||
// CHARGE LEVEL
|
||||
if (data.TryGetValue("SOC", out var soc))
|
||||
{
|
||||
if (Math.Abs(soc - _cachedChargeLevel) > 0.1)
|
||||
{
|
||||
_cachedChargeLevel = soc;
|
||||
}
|
||||
}
|
||||
|
||||
// VOLTAGE
|
||||
if (data.TryGetValue("Voltage", out var volt))
|
||||
{
|
||||
if (Math.Abs(volt - _cachedVoltage) > 0.05)
|
||||
{
|
||||
_cachedVoltage = volt;
|
||||
}
|
||||
}
|
||||
|
||||
// CURRENT
|
||||
if (data.TryGetValue("Current", out var curr))
|
||||
{
|
||||
if (Math.Abs(curr - _cachedCurrent) > 0.05)
|
||||
{
|
||||
_cachedCurrent = curr;
|
||||
}
|
||||
}
|
||||
|
||||
// CHARGING
|
||||
bool newCharging = _cachedCurrent > 0;
|
||||
if (newCharging != _cachedCharging)
|
||||
{
|
||||
_cachedCharging = newCharging;
|
||||
}
|
||||
|
||||
// TEMPERATURE
|
||||
if (data.TryGetValue("Temp", out var t))
|
||||
{
|
||||
if (_cachedTemperature == null || Math.Abs(t - _cachedTemperature.Value) > 0.1)
|
||||
{
|
||||
_cachedTemperature = t;
|
||||
}
|
||||
}
|
||||
|
||||
// Optional fields (per document)
|
||||
_cachedHealth = data.TryGetValue("SOH", out var soh) ? soh : null;
|
||||
_cachedRemainingCapacity = data.TryGetValue("RemainCapacity", out var rc) ? rc : null;
|
||||
_cachedFullCapacity = data.TryGetValue("RemainEnergy", out var re) ? re : null;
|
||||
|
||||
_cachedChargeTime = data.TryGetValue("ChargeTime", out var ct) ? (int)ct : null;
|
||||
_cachedDischargeTime = data.TryGetValue("DischargeTime", out var dt) ? (int)dt : null;
|
||||
_cachedStatusRaw = data.TryGetValue("Status", out var st) ? (int)st : null;
|
||||
|
||||
_lastUpdateTime = now;
|
||||
|
||||
// Update cached BatteryState
|
||||
_cachedBatteryState = CreateBatteryStateFromCache();
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
}
|
||||
|
||||
private void ResetCache()
|
||||
{
|
||||
_cachedChargeLevel = 0;
|
||||
_cachedVoltage = 0;
|
||||
_cachedCurrent = 0;
|
||||
_cachedCharging = false;
|
||||
|
||||
_cachedHealth = null;
|
||||
_cachedTemperature = null;
|
||||
_cachedRemainingCapacity = null;
|
||||
_cachedFullCapacity = null;
|
||||
|
||||
_cachedChargeTime = null;
|
||||
_cachedDischargeTime = null;
|
||||
_cachedStatusRaw = null;
|
||||
|
||||
_lastUpdateTime = DateTime.MinValue;
|
||||
_cachedBatteryState = null;
|
||||
}
|
||||
|
||||
// IBattery Implementation
|
||||
public Task<BatteryState> ReadBatteryStateAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_cachedBatteryState.HasValue)
|
||||
{
|
||||
return Task.FromResult(_cachedBatteryState.Value);
|
||||
}
|
||||
|
||||
return Task.FromResult(CreateBatteryStateFromCache());
|
||||
}
|
||||
}
|
||||
|
||||
// Helper method to create BatteryState from cached values
|
||||
private BatteryState CreateBatteryStateFromCache()
|
||||
{
|
||||
// Map PowerSupplyStatus from charging state
|
||||
byte powerSupplyStatus = BatteryState.PowerSupplyStatusUnknown;
|
||||
if (_cachedCharging)
|
||||
powerSupplyStatus = BatteryState.PowerSupplyStatusCharging;
|
||||
else if (_cachedCurrent < 0)
|
||||
powerSupplyStatus = BatteryState.PowerSupplyStatusDischarging;
|
||||
else if (_cachedCurrent == 0)
|
||||
powerSupplyStatus = BatteryState.PowerSupplyStatusNotCharging;
|
||||
|
||||
// Map PowerSupplyHealth from health percentage
|
||||
byte powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
|
||||
if (_cachedHealth.HasValue)
|
||||
{
|
||||
if (_cachedHealth.Value >= 80)
|
||||
powerSupplyHealth = BatteryState.PowerSupplyHealthGood;
|
||||
else if (_cachedHealth.Value < 20)
|
||||
powerSupplyHealth = BatteryState.PowerSupplyHealthDead;
|
||||
}
|
||||
|
||||
return new BatteryState
|
||||
{
|
||||
Header = new Header
|
||||
{
|
||||
Stamp = _lastUpdateTime != DateTime.MinValue ? _lastUpdateTime : DateTime.UtcNow,
|
||||
FrameId = "battery_frame"
|
||||
},
|
||||
Voltage = _cachedVoltage,
|
||||
Current = _cachedCurrent,
|
||||
Charge = _cachedRemainingCapacity.HasValue ? _cachedRemainingCapacity.Value : double.NaN,
|
||||
Capacity = _cachedFullCapacity.HasValue ? _cachedFullCapacity.Value : double.NaN,
|
||||
DesignCapacity = double.NaN, // Not provided by TADA BMU
|
||||
Percentage = _cachedChargeLevel,
|
||||
PowerSupplyStatus = powerSupplyStatus,
|
||||
PowerSupplyHealth = powerSupplyHealth,
|
||||
PowerSupplyTechnology = BatteryState.PowerSupplyTechnologyUnknown, // Not provided by TADA BMU
|
||||
Present = true,
|
||||
CellVoltage = Array.Empty<double>(), // Not provided by TADA BMU
|
||||
CellTemperature = _cachedTemperature.HasValue ? new[] { _cachedTemperature.Value } : Array.Empty<double>(),
|
||||
Location = string.Empty,
|
||||
SerialNumber = string.Empty
|
||||
};
|
||||
}
|
||||
|
||||
// UpdateProperties
|
||||
private void UpdateProperties()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
SetProperty("ChargeLevel", _cachedChargeLevel.ToString("F1"));
|
||||
SetProperty("Voltage", _cachedVoltage.ToString("F2"));
|
||||
SetProperty("Current", _cachedCurrent.ToString("F2"));
|
||||
SetProperty("Charging", _cachedCharging.ToString());
|
||||
SetProperty("Temperature", _cachedTemperature?.ToString("F1") ?? "0");
|
||||
SetProperty("Health", _cachedHealth?.ToString("F0") ?? "0");
|
||||
SetProperty("RemainCapacity", _cachedRemainingCapacity?.ToString("F2") ?? "0");
|
||||
SetProperty("FullCapacity", _cachedFullCapacity?.ToString("F2") ?? "0");
|
||||
|
||||
SetProperty("ChargeTime", _cachedChargeTime?.ToString() ?? "0");
|
||||
SetProperty("DischargeTime", _cachedDischargeTime?.ToString() ?? "0");
|
||||
SetProperty("StatusRaw", _cachedStatusRaw?.ToString() ?? "0");
|
||||
}
|
||||
}
|
||||
|
||||
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
yield return new PropertyDescription("ChargeLevel", "Charge Level (%)", "Mức pin (%)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 1,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Voltage", "Voltage (V)", "Điện áp (V)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 2,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Current", "Current (A)", "Dòng điện (A)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 3,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Charging", "Charging", "Đang sạc?")
|
||||
{
|
||||
DataType = "boolean",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 4,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiệt độ")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 5,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Health", "SOH (%)", "Sức khỏe pin")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 6,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("RemainCapacity", "Remaining Capacity (Ah)", "Dung lượng còn lại")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 7,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("FullCapacity", "Remaining Energy (Wh)", "Năng lượng còn lại (Wh)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 8,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("ChargeTime", "Charge Time (min)", "Thời gian còn lại để sạc đầy")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 9,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("DischargeTime", "Discharge Time (min)", "Thời gian còn lại để xả hết")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 10,
|
||||
Category = "Status"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("StatusRaw", "Status Flags", "Trạng thái bit (BMU Flags)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 11,
|
||||
Category = "Status"
|
||||
};
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
StopPollingLoop();
|
||||
_client?.Dispose();
|
||||
}
|
||||
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user