QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

593
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/VartaBattery.cs Normal file
View File

@@ -0,0 +1,593 @@
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);
}
}