594 lines
20 KiB
C#
594 lines
20 KiB
C#
using System.Timers;
|
|
using RobotNet10.CANOpen;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.Shared;
|
|
using RobotNet10.Shared.Sensor;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
|
|
|
|
[Device(DeviceType.Battery, "Varta", "VartaBattery", "1.0.0", Description = "Battery Varta CAN Driver")]
|
|
public class VartaBattery : DeviceBase, IBattery
|
|
{
|
|
private readonly ILogger<VartaBattery> _logger;
|
|
private readonly ICanOpenManager _canOpenManager;
|
|
private readonly Lock _dataLock = new();
|
|
|
|
private VartaCanClient? _client;
|
|
private System.Timers.Timer? _pollingTimer;
|
|
|
|
private readonly string _canInterface;
|
|
private readonly int _readTimeoutMs;
|
|
private readonly int _pollingIntervalMs;
|
|
private readonly int _connectionTimeoutMs;
|
|
|
|
private double _cachedChargeLevel;
|
|
private double _cachedVoltage;
|
|
private double _cachedCurrent;
|
|
private bool _cachedCharging;
|
|
|
|
private double? _cachedFetTemperature;
|
|
private double? _cachedCellTemperature;
|
|
private double? _cachedChargeReqVoltage;
|
|
private double? _cachedChargeReqCurrent;
|
|
|
|
private double? _cachedNominalCapacityMah;
|
|
private double? _cachedFullCapacityMah;
|
|
private double? _cachedRemainingCapacityMah;
|
|
private double? _cachedHealth;
|
|
|
|
private int? _cachedInfo;
|
|
private int? _cachedWarn;
|
|
private int? _cachedError;
|
|
private int? _cachedChargeCtrl;
|
|
|
|
private DateTime _lastUpdateTime = DateTime.MinValue;
|
|
private DateTime _connectedAt = DateTime.MinValue;
|
|
private bool _connectionLossSignaled;
|
|
private BatteryState? _cachedBatteryState;
|
|
|
|
public BatteryState? CurrentBatteryState
|
|
{
|
|
get { lock (_dataLock) { return _cachedBatteryState; } }
|
|
}
|
|
|
|
public VartaBattery(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
|
|
: base(deviceId, deviceName, DeviceType.Battery)
|
|
{
|
|
var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
|
|
_logger = loggerFactory.CreateLogger<VartaBattery>();
|
|
_canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
|
|
|
|
_canInterface = connection.GetValue<string>("CanInterface")
|
|
?? connection.GetValue<string>("Interface")
|
|
?? "can0";
|
|
|
|
_readTimeoutMs = connection.GetValue<int?>("ReadTimeoutMs") ?? 200;
|
|
_pollingIntervalMs = connection.GetValue<int?>("PollingIntervalMs") ?? 500;
|
|
_connectionTimeoutMs = connection.GetValue<int?>("ConnectionTimeoutMs")
|
|
?? Math.Max(5000, _pollingIntervalMs * 10);
|
|
|
|
AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true;
|
|
ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 2000;
|
|
MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0;
|
|
|
|
UpdateProperties();
|
|
}
|
|
|
|
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_client?.Dispose();
|
|
_client = new VartaCanClient(_logger, _canOpenManager, _canInterface, _readTimeoutMs);
|
|
}
|
|
|
|
UpdateProperties();
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected override Task OnConnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_client == null)
|
|
{
|
|
_client = new VartaCanClient(_logger, _canOpenManager, _canInterface, _readTimeoutMs);
|
|
}
|
|
else
|
|
{
|
|
_client.ForceReconnect();
|
|
}
|
|
|
|
_connectedAt = DateTime.UtcNow;
|
|
_connectionLossSignaled = false;
|
|
}
|
|
|
|
StartPollingLoop();
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
StopPollingLoop();
|
|
lock (_dataLock)
|
|
{
|
|
_connectionLossSignaled = false;
|
|
_connectedAt = DateTime.MinValue;
|
|
}
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
StopPollingLoop();
|
|
|
|
lock (_dataLock)
|
|
{
|
|
_client?.ForceReconnect();
|
|
ResetCache();
|
|
_connectedAt = DateTime.UtcNow;
|
|
_connectionLossSignaled = false;
|
|
}
|
|
|
|
UpdateProperties();
|
|
StartPollingLoop();
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_client == null || _client.IsFaulted)
|
|
{
|
|
return Task.FromResult(false);
|
|
}
|
|
|
|
var referenceTime = _lastUpdateTime != DateTime.MinValue
|
|
? _lastUpdateTime
|
|
: _connectedAt;
|
|
|
|
if (referenceTime == DateTime.MinValue)
|
|
{
|
|
return Task.FromResult(false);
|
|
}
|
|
|
|
var isRecent = (DateTime.UtcNow - referenceTime).TotalMilliseconds <= _connectionTimeoutMs;
|
|
return Task.FromResult(isRecent);
|
|
}
|
|
}
|
|
|
|
private void StartPollingLoop()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_pollingTimer != null && _pollingTimer.Enabled)
|
|
{
|
|
return;
|
|
}
|
|
|
|
StopPollingLoop();
|
|
|
|
_pollingTimer = new System.Timers.Timer(_pollingIntervalMs)
|
|
{
|
|
AutoReset = true,
|
|
Enabled = true
|
|
};
|
|
_pollingTimer.Elapsed += PollTimer_Elapsed;
|
|
}
|
|
}
|
|
|
|
private void StopPollingLoop()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_pollingTimer == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_pollingTimer.Stop();
|
|
_pollingTimer.Elapsed -= PollTimer_Elapsed;
|
|
_pollingTimer.Dispose();
|
|
_pollingTimer = null;
|
|
}
|
|
}
|
|
|
|
private void PollTimer_Elapsed(object? sender, ElapsedEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
var data = _client?.ReadResponse();
|
|
if (data != null)
|
|
{
|
|
UpdateCache(data);
|
|
return;
|
|
}
|
|
|
|
var noDataMs = _lastUpdateTime != DateTime.MinValue
|
|
? (DateTime.UtcNow - _lastUpdateTime).TotalMilliseconds
|
|
: 0;
|
|
_logger.LogWarning("[VartaBattery] No CAN data received (last update {NoDataMs:F0}ms ago, IsFaulted={IsFaulted})",
|
|
noDataMs, _client?.IsFaulted);
|
|
|
|
if (_client?.IsFaulted == true)
|
|
{
|
|
NotifyConnectionLost("Varta CAN socket faulted");
|
|
return;
|
|
}
|
|
|
|
if (_lastUpdateTime != DateTime.MinValue && noDataMs > _connectionTimeoutMs)
|
|
{
|
|
NotifyConnectionLost($"No Varta CAN data for more than {_connectionTimeoutMs}ms");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
NotifyConnectionLost("Varta CAN polling exception", ex);
|
|
}
|
|
}
|
|
|
|
private void NotifyConnectionLost(string reason, Exception? cause = null)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_connectionLossSignaled)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_connectionLossSignaled = true;
|
|
}
|
|
|
|
LastError = cause ?? new TimeoutException(reason);
|
|
OnErrorOccurred(LastError, reason);
|
|
_ = CheckConnectionAsync();
|
|
}
|
|
|
|
private void UpdateCache(Dictionary<string, double> data)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (data.TryGetValue("SOC", out var soc))
|
|
{
|
|
_cachedChargeLevel = soc;
|
|
}
|
|
|
|
if (data.TryGetValue("Voltage", out var voltage))
|
|
{
|
|
_cachedVoltage = voltage;
|
|
}
|
|
|
|
if (data.TryGetValue("Current", out var current))
|
|
{
|
|
_cachedCurrent = current;
|
|
_cachedCharging = current > 0;
|
|
}
|
|
|
|
_cachedFetTemperature = data.TryGetValue("FetTemp", out var fetTemp) ? fetTemp : _cachedFetTemperature;
|
|
_cachedCellTemperature = data.TryGetValue("CellTemp", out var cellTemp) ? cellTemp : _cachedCellTemperature;
|
|
_cachedChargeReqVoltage = data.TryGetValue("ChargeReqVoltage", out var chargeReqVoltage) ? chargeReqVoltage : _cachedChargeReqVoltage;
|
|
_cachedChargeReqCurrent = data.TryGetValue("ChargeReqCurrent", out var chargeReqCurrent) ? chargeReqCurrent : _cachedChargeReqCurrent;
|
|
|
|
_cachedNominalCapacityMah = data.TryGetValue("NominalCapacityMah", out var nominalCap) ? nominalCap : _cachedNominalCapacityMah;
|
|
_cachedFullCapacityMah = data.TryGetValue("FullCapacityMah", out var fullCap) ? fullCap : _cachedFullCapacityMah;
|
|
_cachedRemainingCapacityMah = data.TryGetValue("RemainingCapacityMah", out var remainingCap) ? remainingCap : _cachedRemainingCapacityMah;
|
|
|
|
// If device provides SOH value directly, use it. Otherwise compute from capacities when available
|
|
if (data.TryGetValue("SOH", out var sohVal))
|
|
{
|
|
_cachedHealth = sohVal;
|
|
}
|
|
else if (_cachedFullCapacityMah.HasValue && _cachedNominalCapacityMah.HasValue && _cachedNominalCapacityMah.Value > 0)
|
|
{
|
|
try
|
|
{
|
|
_cachedHealth = (_cachedFullCapacityMah.Value / _cachedNominalCapacityMah.Value) * 100.0;
|
|
}
|
|
catch
|
|
{
|
|
_cachedHealth = null;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_cachedHealth = null;
|
|
}
|
|
|
|
_cachedInfo = data.TryGetValue("Info", out var info) ? (int)info : _cachedInfo;
|
|
_cachedWarn = data.TryGetValue("Warn", out var warn) ? (int)warn : _cachedWarn;
|
|
_cachedError = data.TryGetValue("Error", out var error) ? (int)error : _cachedError;
|
|
_cachedChargeCtrl = data.TryGetValue("ChargeCtrl", out var chargeCtrl) ? (int)chargeCtrl : _cachedChargeCtrl;
|
|
|
|
_lastUpdateTime = DateTime.UtcNow;
|
|
_connectionLossSignaled = false;
|
|
_cachedBatteryState = CreateBatteryStateFromCache();
|
|
|
|
UpdateProperties();
|
|
}
|
|
}
|
|
|
|
private void ResetCache()
|
|
{
|
|
_cachedChargeLevel = 0;
|
|
_cachedVoltage = 0;
|
|
_cachedCurrent = 0;
|
|
_cachedCharging = false;
|
|
|
|
_cachedFetTemperature = null;
|
|
_cachedCellTemperature = null;
|
|
_cachedChargeReqVoltage = null;
|
|
_cachedChargeReqCurrent = null;
|
|
|
|
_cachedNominalCapacityMah = null;
|
|
_cachedFullCapacityMah = null;
|
|
_cachedRemainingCapacityMah = null;
|
|
|
|
_cachedInfo = null;
|
|
_cachedWarn = null;
|
|
_cachedError = null;
|
|
_cachedChargeCtrl = null;
|
|
|
|
_lastUpdateTime = DateTime.MinValue;
|
|
_connectedAt = DateTime.MinValue;
|
|
_connectionLossSignaled = false;
|
|
_cachedBatteryState = null;
|
|
}
|
|
|
|
public Task<BatteryState> ReadBatteryStateAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_cachedBatteryState.HasValue)
|
|
{
|
|
return Task.FromResult(_cachedBatteryState.Value);
|
|
}
|
|
|
|
return Task.FromResult(CreateBatteryStateFromCache());
|
|
}
|
|
}
|
|
|
|
private BatteryState CreateBatteryStateFromCache()
|
|
{
|
|
byte powerSupplyStatus = BatteryState.PowerSupplyStatusUnknown;
|
|
if (_cachedCharging)
|
|
{
|
|
powerSupplyStatus = BatteryState.PowerSupplyStatusCharging;
|
|
}
|
|
else if (_cachedCurrent < 0)
|
|
{
|
|
powerSupplyStatus = BatteryState.PowerSupplyStatusDischarging;
|
|
}
|
|
else if (_cachedCurrent == 0)
|
|
{
|
|
powerSupplyStatus = BatteryState.PowerSupplyStatusNotCharging;
|
|
}
|
|
|
|
var cellTemperature = _cachedCellTemperature.HasValue
|
|
? new[] { _cachedCellTemperature.Value }
|
|
: Array.Empty<double>();
|
|
|
|
// Map PowerSupplyHealth from computed SOH where possible
|
|
byte powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
|
|
if (_cachedHealth.HasValue)
|
|
{
|
|
if (_cachedHealth.Value >= 80.0)
|
|
powerSupplyHealth = BatteryState.PowerSupplyHealthGood;
|
|
else if (_cachedHealth.Value < 20.0)
|
|
powerSupplyHealth = BatteryState.PowerSupplyHealthDead;
|
|
else
|
|
powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
|
|
}
|
|
|
|
return new BatteryState
|
|
{
|
|
Header = new Header
|
|
{
|
|
Stamp = _lastUpdateTime != DateTime.MinValue ? _lastUpdateTime : DateTime.UtcNow,
|
|
FrameId = "battery_frame"
|
|
},
|
|
Voltage = (float)_cachedVoltage,
|
|
Current = (float)_cachedCurrent,
|
|
Charge = _cachedRemainingCapacityMah.HasValue ? (float)(_cachedRemainingCapacityMah.Value / 1000.0) : float.NaN,
|
|
Capacity = _cachedFullCapacityMah.HasValue ? (float)(_cachedFullCapacityMah.Value / 1000.0) : float.NaN,
|
|
DesignCapacity = _cachedNominalCapacityMah.HasValue ? (float)(_cachedNominalCapacityMah.Value / 1000.0) : float.NaN,
|
|
Percentage = (float)_cachedChargeLevel,
|
|
PowerSupplyStatus = powerSupplyStatus,
|
|
PowerSupplyHealth = powerSupplyHealth,
|
|
PowerSupplyTechnology = BatteryState.PowerSupplyTechnologyUnknown,
|
|
Present = true,
|
|
CellVoltage = [],
|
|
CellTemperature = cellTemperature,
|
|
Location = string.Empty,
|
|
SerialNumber = string.Empty
|
|
};
|
|
}
|
|
|
|
private void UpdateProperties()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
SetProperty("CanInterface", _canInterface);
|
|
SetProperty("ConnectionTimeoutMs", _connectionTimeoutMs.ToString());
|
|
SetProperty("ChargeLevel", _cachedChargeLevel.ToString("F1"));
|
|
SetProperty("Voltage", _cachedVoltage.ToString("F2"));
|
|
SetProperty("Current", _cachedCurrent.ToString("F2"));
|
|
SetProperty("Charging", _cachedCharging.ToString());
|
|
|
|
SetProperty("FetTemperature", _cachedFetTemperature?.ToString("F1") ?? "0");
|
|
SetProperty("CellTemperature", _cachedCellTemperature?.ToString("F1") ?? "0");
|
|
SetProperty("ChargeReqVoltage", _cachedChargeReqVoltage?.ToString("F2") ?? "0");
|
|
SetProperty("ChargeReqCurrent", _cachedChargeReqCurrent?.ToString("F2") ?? "0");
|
|
|
|
SetProperty("NominalCapacityMah", _cachedNominalCapacityMah?.ToString("F0") ?? "0");
|
|
SetProperty("FullCapacityMah", _cachedFullCapacityMah?.ToString("F0") ?? "0");
|
|
SetProperty("RemainingCapacityMah", _cachedRemainingCapacityMah?.ToString("F0") ?? "0");
|
|
SetProperty("Health", _cachedHealth?.ToString("F0") ?? "0");
|
|
|
|
SetProperty("Info", _cachedInfo?.ToString() ?? "0");
|
|
SetProperty("Warn", _cachedWarn?.ToString() ?? "0");
|
|
SetProperty("Error", _cachedError?.ToString() ?? "0");
|
|
SetProperty("ChargeCtrl", _cachedChargeCtrl?.ToString() ?? "0");
|
|
}
|
|
}
|
|
|
|
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
|
{
|
|
yield return new PropertyDescription("CanInterface", "CAN Interface", "CAN interface của pin")
|
|
{
|
|
DataType = "text",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 1,
|
|
Category = "Config"
|
|
};
|
|
|
|
yield return new PropertyDescription("ConnectionTimeoutMs", "Connection Timeout (ms)", "Ngưỡng timeout phát hiện mất kết nối")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 2,
|
|
Category = "Config"
|
|
};
|
|
|
|
yield return new PropertyDescription("ChargeLevel", "Charge Level (%)", "Mức pin (%)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 3,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Voltage", "Voltage (V)", "Điện áp (V)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 4,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Current", "Current (A)", "Dòng điện (A)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 5,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Charging", "Charging", "Đang sạc?")
|
|
{
|
|
DataType = "boolean",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 6,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("FetTemperature", "FET Temp (C)", "Nhiệt độ FET")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 7,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("CellTemperature", "Cell Temp (C)", "Nhiệt độ cell")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 8,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("ChargeReqVoltage", "Charge Req Voltage (V)", "Điện áp sạc yêu cầu")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 9,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("ChargeReqCurrent", "Charge Req Current (A)", "Dòng sạc yêu cầu")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 10,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("NominalCapacityMah", "Nominal Capacity (mAh)", "Dung lượng danh định")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 11,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("FullCapacityMah", "Full Capacity (mAh)", "Dung lượng đầy")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 12,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("RemainingCapacityMah", "Remaining Capacity (mAh)", "Dung lượng còn lại")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 13,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Health", "SOH (%)", "Sức khỏe pin")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 14,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Info", "Info Flags", "Cờ thông tin")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 15,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Warn", "Warn Flags", "Cờ cảnh báo")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 16,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("Error", "Error Flags", "Cờ lỗi")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 17,
|
|
Category = "Status"
|
|
};
|
|
|
|
yield return new PropertyDescription("ChargeCtrl", "Charge Control", "Trạng thái điều khiển sạc")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 18,
|
|
Category = "Status"
|
|
};
|
|
}
|
|
|
|
protected override void Dispose(bool disposing)
|
|
{
|
|
if (disposing)
|
|
{
|
|
StopPollingLoop();
|
|
_client?.Dispose();
|
|
}
|
|
|
|
base.Dispose(disposing);
|
|
}
|
|
}
|