429 lines
14 KiB
C#
429 lines
14 KiB
C#
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);
|
|
}
|
|
}
|