Initial commit
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,285 @@
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.CANOpen;
|
||||
using RobotNet10.CANOpen.Interfaces;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
// using
|
||||
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
|
||||
|
||||
/// <summary>
|
||||
/// Varta CAN client — đọc dữ liệu pin qua CANopen PDO.
|
||||
/// Dùng SocketCAN transport có sẵn trong RobotNet10.CANOpen.
|
||||
/// Protocol (CAN 11-bit):
|
||||
/// 0x19B -> Voltage, Current
|
||||
/// 0x281 -> FetTemp, CellTemp, ChargeReqVoltage, ChargeReqCurrent
|
||||
/// 0x381 -> NominalCapacity, FullCapacity, RemainingCapacity, SOC
|
||||
/// 0x481/0x581 -> Info, Warn, Error, ChargeCtrl
|
||||
/// </summary>
|
||||
public sealed class VartaCanClient : IDisposable
|
||||
{
|
||||
private readonly ILogger _logger;
|
||||
private readonly ICanOpenManager _canOpenManager;
|
||||
private readonly string _canInterface;
|
||||
private readonly int _readTimeoutMs;
|
||||
private readonly Lock _stateLock = new();
|
||||
private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
|
||||
private readonly SemaphoreSlim _rxSignal = new(0);
|
||||
private ICanBus? _bus;
|
||||
private bool _disposed;
|
||||
private bool _isFaulted;
|
||||
|
||||
public bool IsFaulted
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _isFaulted;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public VartaCanClient(ILogger logger, ICanOpenManager canOpenManager, string canInterface, int readTimeoutMs = 200)
|
||||
{
|
||||
_logger = logger;
|
||||
_canOpenManager = canOpenManager;
|
||||
_canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
|
||||
_readTimeoutMs = Math.Max(10, readTimeoutMs);
|
||||
|
||||
OpenBus();
|
||||
}
|
||||
|
||||
|
||||
private void OpenBus()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_isFaulted = false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// Xóa bus cũ khỏi cache của CanOpenManager trước khi tạo lại,
|
||||
// tránh GetOrCreateCanBusAsync trả về bus đã chết do caching.
|
||||
_logger.LogInformation("[VartaCanClient] Removing old CAN bus from cache for {Iface}", _canInterface);
|
||||
_canOpenManager.RemoveCanBusAsync(_canInterface).GetAwaiter().GetResult();
|
||||
|
||||
var bus = _canOpenManager.GetOrCreateCanBusAsync(_canInterface).GetAwaiter().GetResult();
|
||||
_logger.LogInformation("[VartaCanClient] CAN bus recreated for {Iface}, IsConnected={IsConnected}", _canInterface, bus.IsConnected);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (_bus != null)
|
||||
{
|
||||
_bus.FrameReceived -= OnFrameReceived;
|
||||
}
|
||||
|
||||
_bus = bus;
|
||||
_bus.FrameReceived += OnFrameReceived;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_isFaulted = true;
|
||||
}
|
||||
// _logger.LogError(ex, "[VartaCanClient] Không thể mở SocketCAN trên interface {Iface}", _canInterface);
|
||||
}
|
||||
}
|
||||
|
||||
public void ForceReconnect()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("[VartaCanClient] ForceReconnect start {Iface}", _canInterface);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_bus != null)
|
||||
{
|
||||
_bus.FrameReceived -= OnFrameReceived;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "[VartaCanClient] No data in {Iface}", _canInterface);
|
||||
}
|
||||
|
||||
while (_rxQueue.TryDequeue(out _)) { }
|
||||
while (_rxSignal.Wait(0)) { }
|
||||
}
|
||||
|
||||
OpenBus();
|
||||
_logger.LogInformation("[VartaCanClient] ForceReconnect Finished, IsFaulted={IsFaulted}", _isFaulted);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Đọc frame mới nhất từ queue receive và decode theo protocol Varta.
|
||||
/// Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID để tránh trễ dữ liệu.
|
||||
/// </summary>
|
||||
public Dictionary<string, double>? ReadResponse(int maxFrames = 30)
|
||||
{
|
||||
if (_disposed || IsFaulted || _bus == null || !_bus.IsConnected)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Nếu queue rỗng, chờ frame mới đến
|
||||
if (_rxQueue.IsEmpty)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!_rxSignal.Wait(_readTimeoutMs))
|
||||
{
|
||||
return null;
|
||||
ForceReconnect();
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
// Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID
|
||||
var latestFrames = new Dictionary<uint, CanFrameReceivedEventArgs>();
|
||||
while (_rxQueue.TryDequeue(out var frame))
|
||||
{
|
||||
latestFrames[frame.CanId] = frame;
|
||||
// Drain semaphore để khớp với số frame bị loại bỏ
|
||||
_rxSignal.Wait(0);
|
||||
}
|
||||
|
||||
if (latestFrames.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var result = new Dictionary<string, double>(StringComparer.OrdinalIgnoreCase);
|
||||
foreach (var frame in latestFrames.Values)
|
||||
{
|
||||
DecodeFrame(frame, result);
|
||||
}
|
||||
|
||||
return result.Count == 0 ? null : result;
|
||||
}
|
||||
|
||||
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_rxQueue.Enqueue(e);
|
||||
try
|
||||
{
|
||||
_rxSignal.Release();
|
||||
}
|
||||
catch (SemaphoreFullException)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private static void DecodeFrame(CanFrameReceivedEventArgs frame, Dictionary<string, double> result)
|
||||
{
|
||||
// uint canId = frame.CanId & 0x7FFu;
|
||||
var d = frame.Data;
|
||||
|
||||
if (d == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// var canBase = canId & 0x780u;
|
||||
|
||||
switch (frame.CanId)
|
||||
{
|
||||
case 0x181: // TPDO1: 0x180 + NodeId
|
||||
{
|
||||
uint voltage = BitConverter.ToUInt32(d, 0);
|
||||
int current = BitConverter.ToInt32(d, 4);
|
||||
double volts = voltage / 1000.0;
|
||||
double amps = current / 1000.0;
|
||||
result["Voltage"] = Math.Round(volts, 1, MidpointRounding.ToZero);
|
||||
result["Current"] = Math.Round(amps, 1, MidpointRounding.ToZero);
|
||||
// Console.WriteLine($"[VartaCanClient] Received TPDO1: Voltage={volts} V, Current={amps} A, Timestamps: {DateTime.Now:HH:mm:ss.fff} s");
|
||||
break;
|
||||
}
|
||||
|
||||
case 0x281: // TPDO2: 0x280 + NodeId
|
||||
result["FetTemp"] = BitConverter.ToInt16(d, 0) / 10.0;
|
||||
result["CellTemp"] = BitConverter.ToInt16(d, 2) / 10.0;
|
||||
result["ChargeReqVoltage"] = BitConverter.ToUInt16(d, 4) / 1000.0;
|
||||
result["ChargeReqCurrent"] = BitConverter.ToUInt16(d, 6) / 1000.0;
|
||||
// Console.WriteLine($"[VartaCanClient] Received TPDO2: FetTemp={result["FetTemp"]} °C, CellTemp={result["CellTemp"]} °C, ChargeReqVoltage={result["ChargeReqVoltage"]} V, ChargeReqCurrent={result["ChargeReqCurrent"]} A");
|
||||
break;
|
||||
|
||||
case 0x381: // TPDO3: 0x380 + NodeId
|
||||
{
|
||||
var nominal = BitConverter.ToUInt16(d, 0);
|
||||
var full = BitConverter.ToUInt16(d, 2);
|
||||
var remaining = BitConverter.ToUInt16(d, 4);
|
||||
result["NominalCapacityMah"] = nominal;
|
||||
result["FullCapacityMah"] = full;
|
||||
result["RemainingCapacityMah"] = remaining;
|
||||
result["SOC"] = full == 0 ? 0 : remaining * 100.0 / full;
|
||||
result["SOH"] = nominal == 0 ? 0 : full * 100.0 / nominal;
|
||||
// Console.WriteLine($"[VartaCanClient] Received TPDO3: Nominal={nominal} mAh, Full={full} mAh, Remaining={remaining} mAh, SOC={result["SOC"]} %, SOH={result["SOH"]} %");
|
||||
break;
|
||||
}
|
||||
|
||||
case 0x481: // TPDO4: 0x480 + NodeId
|
||||
|
||||
case 0x581: // SDO response: 0x580 + NodeId (some firmware puts status words here)
|
||||
result["Info"] = BitConverter.ToUInt16(d, 0);
|
||||
result["Warn"] = BitConverter.ToUInt16(d, 2);
|
||||
result["Error"] = BitConverter.ToUInt16(d, 4);
|
||||
result["ChargeCtrl"] = BitConverter.ToUInt16(d, 6);
|
||||
// Console.WriteLine($"[VartaCanClient] Received TPDO4/SDO: Info={result["Info"]}, Warn={result["Warn"]}, Error={result["Error"]}, ChargeCtrl={result["ChargeCtrl"]}");
|
||||
break;
|
||||
|
||||
case 0x264:
|
||||
result["ChargeControl"] = d[0]; // byte 0: uint8
|
||||
result["SOC"] = d[1]; // byte 1: uint8, %
|
||||
// byte 2: không sử dụng
|
||||
result["ChargeVoltageRequest"] = BitConverter.ToUInt16(d, 3) / 256.0; // bytes 3-4: uint16, 1/256 V
|
||||
result["ChargeCurrentRequest"] = BitConverter.ToUInt16(d, 5) / 16.0; // bytes 5-6: uint16, 1/16 A
|
||||
result["BatteryStatus"] = d[7]; // byte 7: uint8
|
||||
// Console.WriteLine($"[VartaCanClient] Received 0x264: ChargeControl={result["ChargeControl"]}, SOC={result["SOC"]} %, ChargeVoltageRequest={result["ChargeVoltageRequest"]:F4} V, ChargeCurrentRequest={result["ChargeCurrentRequest"]:F4} A, BatteryStatus={result["BatteryStatus"]}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_bus?.FrameReceived -= OnFrameReceived;
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
finally
|
||||
{
|
||||
_rxSignal.Dispose();
|
||||
while (_rxQueue.TryDequeue(out _)) { }
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.CANOpen;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
|
||||
|
||||
public sealed class VartaChargerSimulatorService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly ILogger<VartaChargerSimulatorService> _logger;
|
||||
private readonly ICanOpenManager _canOpenManager;
|
||||
private readonly VartaChargerSimulatorOptions _options;
|
||||
private readonly SemaphoreSlim _controlLock = new(1, 1);
|
||||
|
||||
private Task? _runTask;
|
||||
private CancellationTokenSource? _runCts;
|
||||
private string _currentInterface = "can0";
|
||||
private bool _disposed;
|
||||
|
||||
public bool IsRunning => _runTask is { IsCompleted: false };
|
||||
public string CurrentInterface => _currentInterface;
|
||||
|
||||
public VartaChargerSimulatorService(
|
||||
IConfiguration configuration,
|
||||
ILogger<VartaChargerSimulatorService> logger,
|
||||
ICanOpenManager canOpenManager)
|
||||
{
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
_canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
|
||||
|
||||
_options = new VartaChargerSimulatorOptions();
|
||||
configuration.GetSection("Varta:Charger59VSimulator").Bind(_options);
|
||||
}
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_options.Enabled)
|
||||
{
|
||||
await StartSimulatorAsync(_options.CanInterface, cancellationToken);
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("Varta Charger59V simulator is disabled at startup.");
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
await StopSimulatorAsync(cancellationToken);
|
||||
}
|
||||
|
||||
public async Task<bool> StartSimulatorAsync(string? canInterface = null, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ThrowIfDisposed();
|
||||
|
||||
await _controlLock.WaitAsync(cancellationToken);
|
||||
try
|
||||
{
|
||||
if (_runTask is { IsCompleted: false })
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
_currentInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
|
||||
_runCts = new CancellationTokenSource();
|
||||
_runTask = RunSimulatorAsync(_currentInterface, _runCts.Token);
|
||||
return true;
|
||||
}
|
||||
finally
|
||||
{
|
||||
_controlLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<bool> StopSimulatorAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ThrowIfDisposed();
|
||||
|
||||
Task? runTask;
|
||||
CancellationTokenSource? runCts;
|
||||
|
||||
await _controlLock.WaitAsync(cancellationToken);
|
||||
try
|
||||
{
|
||||
if (_runTask is not { IsCompleted: false } || _runCts is null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
runTask = _runTask;
|
||||
runCts = _runCts;
|
||||
_runTask = null;
|
||||
_runCts = null;
|
||||
}
|
||||
finally
|
||||
{
|
||||
_controlLock.Release();
|
||||
}
|
||||
|
||||
runCts.Cancel();
|
||||
try
|
||||
{
|
||||
await runTask.WaitAsync(cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
}
|
||||
finally
|
||||
{
|
||||
runCts.Dispose();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private async Task RunSimulatorAsync(string canInterface, CancellationToken token)
|
||||
{
|
||||
_logger.LogInformation("Starting Varta Charger59V simulator on CAN interface {CanInterface}", canInterface);
|
||||
|
||||
try
|
||||
{
|
||||
var simulator = new Charger59V(_canOpenManager, canInterface, token);
|
||||
await simulator.RunAsync();
|
||||
}
|
||||
catch (OperationCanceledException) when (token.IsCancellationRequested)
|
||||
{
|
||||
_logger.LogInformation("Varta Charger59V simulator stopped.");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Varta Charger59V simulator crashed.");
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_runCts?.Cancel();
|
||||
_runCts?.Dispose();
|
||||
_controlLock.Dispose();
|
||||
}
|
||||
|
||||
private void ThrowIfDisposed()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
throw new ObjectDisposedException(nameof(VartaChargerSimulatorService));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class VartaChargerSimulatorOptions
|
||||
{
|
||||
public bool Enabled { get; set; }
|
||||
public string CanInterface { get; set; } = "can0";
|
||||
}
|
||||
|
||||
public sealed class VartaChargerSimulatorStartRequest
|
||||
{
|
||||
public string? CanInterface { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,488 @@
|
||||
using RobotNet10.CANOpen;
|
||||
using RobotNet10.CANOpen.Interfaces;
|
||||
using System.Collections.Concurrent;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
|
||||
/// <summary>
|
||||
/// Charger Simulator cho VARTA EasyBlade 59V
|
||||
/// Máy tính đóng vai Charger, giao tiếp với pin thật qua USB-CAN adapter.
|
||||
///
|
||||
/// ── Thông số cố định (theo Technical Spec V1.8) ──────────────────────
|
||||
/// Baud rate : 250 kbit/s
|
||||
/// Charger Node ID : 100 (0x64)
|
||||
/// Max voltage : 58.8 V (4 × 12V × 1.225 cells)
|
||||
/// Max current : 25 A
|
||||
/// Heartbeat : mỗi 1000 ms → COB-ID 0x764
|
||||
/// RPDO1 : mỗi 200 ms → COB-ID 0x1E4
|
||||
/// ─────────────────────────────────────────────────────────────────────
|
||||
/// </summary>
|
||||
public class Charger59V
|
||||
{
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// ⚙️ THÔNG SỐ HARDCODE – CHỈNH TẠI ĐÂY NẾU CẦN
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
// Điện áp tối đa charger có thể cung cấp (V)
|
||||
// EasyBlade 59V: pin lithium 13S → max 54.6V, để an toàn dùng 58.8V
|
||||
private const double MAX_VOLTAGE_V = 58.8;
|
||||
|
||||
// Dòng tối đa charger có thể cung cấp (A)
|
||||
private const double MAX_CURRENT_A = 25.0;
|
||||
|
||||
// Điện áp thực đo được (báo lại pin trong RPDO1, byte 2-3)
|
||||
// Lúc chưa sạc thực thì đặt bằng Max hoặc giá trị đo thực của nguồn
|
||||
private const double ACTUAL_VOLTAGE_V = 54.0;
|
||||
|
||||
// Dòng thực đo được (báo lại pin trong RPDO1, byte 0-1)
|
||||
private const double ACTUAL_CURRENT_A = 10.0;
|
||||
|
||||
// COB-ID (không đổi theo spec)
|
||||
private const uint COB_HEARTBEAT = 0x764; // gửi
|
||||
private const uint COB_RPDO1 = 0x1E4; // gửi
|
||||
private const uint COB_SDO_TX = 0x5E4; // gửi (response về battery)
|
||||
private const uint COB_SDO_RX = 0x664; // nhận (request từ battery)
|
||||
private const uint COB_TPDO9 = 0x264; // nhận (SoC, VReq, IReq)
|
||||
private const uint COB_TPDO8 = 0x49B; // nhận (charge control status)
|
||||
|
||||
// ── Giá trị raw (Q8 = ×256, Q4 = ×16) ───────────────────────────
|
||||
private static readonly ushort RAW_MAX_VOLTAGE = (ushort)(MAX_VOLTAGE_V * 256);
|
||||
private static readonly ushort RAW_MAX_CURRENT = (ushort)(MAX_CURRENT_A * 16);
|
||||
private static readonly ushort RAW_ACT_VOLTAGE = (ushort)(ACTUAL_VOLTAGE_V * 256);
|
||||
private static readonly ushort RAW_ACT_CURRENT = (ushort)(ACTUAL_CURRENT_A * 256);
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// State
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private bool _sdoInitDone = false;
|
||||
private bool _chargeActive = false; // true sau khi set Bit12
|
||||
private bool _relayOpen = true; // true = relay mở, không có điện ra
|
||||
private bool _batteryCharging = false; // true khi pin báo đang vào trạng thái sạc
|
||||
|
||||
// Lưu lại giá trị SDO battery ghi vào charger
|
||||
private byte _batteryStatus = 0; // Object 0x6000
|
||||
private byte _chargeControl = 0; // Object 0x4200
|
||||
private ushort _voltageReqRaw = 0; // Object 0x2276
|
||||
private ushort _currentReqRaw = 0; // Object 0x6070
|
||||
|
||||
private readonly ICanOpenManager _canOpenManager;
|
||||
private readonly string _canInterface;
|
||||
private ICanBus? _can;
|
||||
private readonly CancellationToken _ct;
|
||||
private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
|
||||
private readonly SemaphoreSlim _rxSignal = new(0);
|
||||
|
||||
public Charger59V(ICanOpenManager canOpenManager, string canInterface, CancellationToken ct)
|
||||
{
|
||||
_canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
|
||||
_canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
|
||||
_ct = ct;
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
public async Task RunAsync()
|
||||
{
|
||||
_can = await _canOpenManager.GetOrCreateCanBusAsync(_canInterface, _ct);
|
||||
_can.FrameReceived += OnFrameReceived;
|
||||
|
||||
// Log($"Max Voltage : {MAX_VOLTAGE_V} V (raw Q8 = {RAW_MAX_VOLTAGE})");
|
||||
// Log($"Max Current : {MAX_CURRENT_A} A (raw Q4 = {RAW_MAX_CURRENT})");
|
||||
// Log($"Gửi Heartbeat 0x{COB_HEARTBEAT:X3} mỗi 1000ms...");
|
||||
// Log("Đang chờ pin kết nối...\n");
|
||||
|
||||
try
|
||||
{
|
||||
// Chạy song song 3 vòng lặp
|
||||
await Task.WhenAll(
|
||||
HeartbeatLoopAsync(), // gửi HB mỗi 1000ms
|
||||
ReceiveLoopAsync(), // nhận SDO + TPDO từ pin
|
||||
Rpdo1LoopAsync() // gửi RPDO1 sau khi SDO init xong
|
||||
);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_can.FrameReceived -= OnFrameReceived;
|
||||
while (_rxQueue.TryDequeue(out _)) { }
|
||||
while (_rxSignal.Wait(0)) { }
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// VÒNG LẶP 1 – Heartbeat (mỗi 1000ms)
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private async Task HeartbeatLoopAsync()
|
||||
{
|
||||
while (!_ct.IsCancellationRequested)
|
||||
{
|
||||
// NMT Heartbeat: 1 byte [0x05] = Operational state
|
||||
SendFrame(COB_HEARTBEAT, [0x05]);
|
||||
// Dim($"♥ HB → 0x{COB_HEARTBEAT:X3}");
|
||||
await Task.Delay(1000, _ct);
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// VÒNG LẶP 2 – Nhận frame từ pin
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private async Task ReceiveLoopAsync()
|
||||
{
|
||||
while (!_ct.IsCancellationRequested)
|
||||
{
|
||||
if (TryReceive(out var frame))
|
||||
{
|
||||
switch (frame.CanId)
|
||||
{
|
||||
case COB_SDO_RX: HandleSdo(frame.Data); break;
|
||||
case COB_TPDO9: HandleTpdo9(frame.Data); break;
|
||||
case COB_TPDO8: HandleTpdo8(frame.Data); break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
await Task.Delay(1, _ct); // yield CPU khi không có frame
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// VÒNG LẶP 3 – Gửi RPDO1 (mỗi 200ms, sau khi SDO init xong)
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private async Task Rpdo1LoopAsync()
|
||||
{
|
||||
// Chờ SDO init hoàn tất
|
||||
while (!_sdoInitDone && !_ct.IsCancellationRequested)
|
||||
await Task.Delay(50, _ct);
|
||||
|
||||
if (_ct.IsCancellationRequested) return;
|
||||
|
||||
// Delay 1s trước khi kích hoạt Bit12 (cho pin ổn định)
|
||||
LogOk("SDO init xong! Chờ 1s rồi bật Bit12...");
|
||||
await Task.Delay(1000, _ct);
|
||||
|
||||
// Bật relay và charge mode
|
||||
_relayOpen = false;
|
||||
_chargeActive = true;
|
||||
LogOk("==> Bit12 SET – Pin đang chuyển sang CHARGE MODE!");
|
||||
|
||||
// Gửi RPDO1 mỗi 200ms
|
||||
while (!_ct.IsCancellationRequested)
|
||||
{
|
||||
SendRpdo1();
|
||||
await Task.Delay(200, _ct);
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// GỬI RPDO1 (COB-ID 0x1E4)
|
||||
//
|
||||
// Byte 0-1: Charging Current [1/256 A, Q8]
|
||||
// Byte 2-3: Charging Voltage [1/256 V, Q8]
|
||||
// Byte 4-5: Max avail Current [1/16 A, Q4]
|
||||
// Byte 6-7: Extended Charger Status
|
||||
// → Bit12 (0x1000) = kích hoạt charge mode
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void SendRpdo1()
|
||||
{
|
||||
ushort extStatus = (_chargeActive && !_relayOpen)
|
||||
? (ushort)0x1000 // Bit12 set
|
||||
: (ushort)0x0000;
|
||||
|
||||
byte[] data =
|
||||
[
|
||||
(byte)(RAW_ACT_CURRENT & 0xFF), (byte)(RAW_ACT_CURRENT >> 8), // Byte 0-1
|
||||
(byte)(RAW_ACT_VOLTAGE & 0xFF), (byte)(RAW_ACT_VOLTAGE >> 8), // Byte 2-3
|
||||
(byte)(RAW_MAX_CURRENT & 0xFF), (byte)(RAW_MAX_CURRENT >> 8), // Byte 4-5
|
||||
(byte)(extStatus & 0xFF), (byte)(extStatus >> 8), // Byte 6-7
|
||||
];
|
||||
|
||||
SendFrame(COB_RPDO1, data);
|
||||
// Dim($"→ RPDO1 0x{COB_RPDO1:X3} [{string.Join(" ", data.Select(b => $"{b:X2}"))}] " +
|
||||
// $"ExtStat=0x{extStatus:X4}");
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// XỬ LÝ SDO REQUEST TỪ PIN (COB-ID 0x664)
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void HandleSdo(byte[] d)
|
||||
{
|
||||
if (d.Length < 8) return;
|
||||
byte cmd = d[0];
|
||||
ushort index = (ushort)(d[1] | (d[2] << 8));
|
||||
byte sub = d[3];
|
||||
|
||||
switch (cmd)
|
||||
{
|
||||
// Pin GHI vào object của charger
|
||||
case 0x2F: // Write 1 byte
|
||||
OnWrite(index, sub, d[4], 0);
|
||||
break;
|
||||
case 0x2B: // Write 2 bytes
|
||||
OnWrite(index, sub, d[4], (ushort)(d[4] | (d[5] << 8)));
|
||||
break;
|
||||
case 0x23: // Write 4 bytes
|
||||
SdoWriteOk(index, sub); // phản hồi OK, bỏ qua giá trị
|
||||
break;
|
||||
|
||||
// Pin ĐỌC object từ charger
|
||||
case 0x40: // Read request
|
||||
OnRead(index, sub);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void OnWrite(ushort index, byte sub, byte val8, ushort val16)
|
||||
{
|
||||
switch (index)
|
||||
{
|
||||
case 0x6000: // Battery Status
|
||||
_batteryStatus = val8;
|
||||
// Log($" [SDO] 0x6000 Battery Status ← {val8} " +
|
||||
// (val8 == 1 ? "→ Relay CLOSED (power ON)" : "→ Relay OPEN (power OFF)"));
|
||||
_relayOpen = (val8 == 0);
|
||||
break;
|
||||
|
||||
case 0x4200: // Charge Control
|
||||
_chargeControl = val8;
|
||||
// Log($" [SDO] 0x4200 Charge Control ← {val8} " +
|
||||
// (val8 == 1 ? "→ Battery READY" : "→ Battery NOT ready"));
|
||||
// ChargeControl=0 → pin báo full/lỗi → tắt relay
|
||||
if (val8 == 0 && _sdoInitDone)
|
||||
{
|
||||
LogWarn("ChargeControl=0 → TẮT RELAY (pin đầy hoặc lỗi)");
|
||||
_relayOpen = true;
|
||||
_chargeActive = false;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x2276: // Voltage Request
|
||||
_voltageReqRaw = val16;
|
||||
// Log($" [SDO] 0x2276 Voltage Request ← {val16 / 256.0:F3} V");
|
||||
break;
|
||||
|
||||
case 0x6070: // Current Request
|
||||
_currentReqRaw = val16;
|
||||
// Log($" [SDO] 0x6070 Current Request ← {val16 / 16.0:F3} A");
|
||||
break;
|
||||
|
||||
default:
|
||||
// Log($" [SDO] Write idx=0x{index:X4}.{sub} val=0x{val16:X4}");
|
||||
break;
|
||||
}
|
||||
SdoWriteOk(index, sub);
|
||||
CheckInitComplete();
|
||||
}
|
||||
|
||||
private void OnRead(ushort index, byte sub)
|
||||
{
|
||||
switch (index)
|
||||
{
|
||||
case 0x4208: // Max Charging Voltage
|
||||
SdoReadOk2(index, sub, RAW_MAX_VOLTAGE);
|
||||
// Log($" [SDO] 0x4208 Max Voltage → {MAX_VOLTAGE_V} V (raw=0x{RAW_MAX_VOLTAGE:X4})");
|
||||
break;
|
||||
|
||||
case 0x4212: // Max Charging Current
|
||||
SdoReadOk2(index, sub, RAW_MAX_CURRENT);
|
||||
// Log($" [SDO] 0x4212 Max Current → {MAX_CURRENT_A} A (raw=0x{RAW_MAX_CURRENT:X4})");
|
||||
break;
|
||||
|
||||
default:
|
||||
// Abort: object does not exist
|
||||
byte[] abort = [0x80,
|
||||
(byte)(index & 0xFF), (byte)(index >> 8), sub,
|
||||
0x00, 0x00, 0x02, 0x06];
|
||||
SendFrame(COB_SDO_TX, abort);
|
||||
break;
|
||||
}
|
||||
CheckInitComplete();
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// XỬ LÝ TPDO9 (COB-ID 0x264) – Pin gửi mỗi 100ms
|
||||
// Byte 0: ChargeControl Byte 1: SoC
|
||||
// Byte 3-4: Volt Request Byte 5-6: Curr Request Byte 7: BattStatus
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void HandleTpdo9(byte[] d)
|
||||
{
|
||||
if (d.Length < 7) return;
|
||||
byte cc = d[0];
|
||||
byte soc = d[1];
|
||||
ushort vReq = (ushort)(d[3] | (d[4] << 8));
|
||||
ushort iReq = (ushort)(d[5] | (d[6] << 8));
|
||||
byte bs = d.Length > 7 ? d[7] : (byte)0;
|
||||
|
||||
Console.ForegroundColor = ConsoleColor.Green;
|
||||
// Console.WriteLine(
|
||||
// $"[{Now}] 📦 PIN " +
|
||||
// $"SoC={soc,3}% " +
|
||||
// $"VReq={vReq / 256.0,6:F2}V " +
|
||||
// $"IReq={iReq / 16.0,6:F2}A " +
|
||||
// $"ChargeCtrl={cc} BattStat={bs}");
|
||||
// Console.ResetColor();
|
||||
|
||||
// Pin gửi ChargeControl=0 → pin đầy hoặc có lỗi → dừng sạc
|
||||
if (cc == 0 && _sdoInitDone && _chargeActive)
|
||||
{
|
||||
LogWarn("ChargeControl=0 → PIN ĐẦY hoặc LỖI → Tắt relay!");
|
||||
_chargeActive = false;
|
||||
_relayOpen = true;
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// XỬ LÝ TPDO8 (COB-ID 0x49B) – Battery Charge Control Status
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void HandleTpdo8(byte[] d)
|
||||
{
|
||||
if (d.Length < 2) return;
|
||||
ushort s = (ushort)(d[0] | (d[1] << 8));
|
||||
bool chargingNow = s == 0xC011 || s == 0xC033;
|
||||
|
||||
if (chargingNow && !_batteryCharging)
|
||||
{
|
||||
_batteryCharging = true;
|
||||
LogOk($"✅ PIN ĐÃ VÀO TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
|
||||
}
|
||||
else if (!chargingNow && _batteryCharging)
|
||||
{
|
||||
_batteryCharging = false;
|
||||
LogWarn($"PIN THOÁT TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
|
||||
}
|
||||
|
||||
string desc = s switch
|
||||
{
|
||||
0x0033 => "SDO init OK – chờ Bit12",
|
||||
0x4033 => "Standby – chờ Bit12",
|
||||
0xC011 => "⚡ CHARGING ACTIVE",
|
||||
0xC033 => "⚡ Charging (normal)",
|
||||
0xC000 => "Pin đầy – về standby",
|
||||
0xD000 => "Keep-power hết – SHUTDOWN",
|
||||
_ => $"bits={s:X4}"
|
||||
};
|
||||
Console.ForegroundColor = ConsoleColor.Cyan;
|
||||
// Console.WriteLine($"[{Now}] 📊 STATUS 0x{s:X4} → {desc}");
|
||||
Console.ResetColor();
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// Kiểm tra SDO init sequence đã đủ 4 bước chưa
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void CheckInitComplete()
|
||||
{
|
||||
if (_sdoInitDone) return;
|
||||
if (_batteryStatus == 1
|
||||
&& _chargeControl == 1
|
||||
&& _voltageReqRaw > 0
|
||||
&& _currentReqRaw > 0)
|
||||
{
|
||||
_sdoInitDone = true;
|
||||
// Console.ForegroundColor = ConsoleColor.Yellow;
|
||||
// Console.WriteLine($"\n[{Now}] ══════════════════════════════════════");
|
||||
// Console.WriteLine($"[{Now}] ✅ SDO INITIALIZATION HOÀN TẤT!");
|
||||
// Console.WriteLine($"[{Now}] BatteryStatus={_batteryStatus} ChargeControl={_chargeControl}");
|
||||
// Console.WriteLine($"[{Now}] VoltReq={_voltageReqRaw / 256.0:F3}V CurrReq={_currentReqRaw / 16.0:F3}A");
|
||||
// Console.WriteLine($"[{Now}] ══════════════════════════════════════\n");
|
||||
// Console.ResetColor();
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// SDO helpers
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private void SdoWriteOk(ushort index, byte sub)
|
||||
{
|
||||
byte[] d = [0x60, (byte)(index & 0xFF), (byte)(index >> 8), sub, 0, 0, 0, 0];
|
||||
SendFrame(COB_SDO_TX, d);
|
||||
// Dim($"← SDO OK 0x{COB_SDO_TX:X3} idx=0x{index:X4}");
|
||||
}
|
||||
|
||||
private void SdoReadOk2(ushort index, byte sub, ushort value)
|
||||
{
|
||||
byte[] d = [0x4B,
|
||||
(byte)(index & 0xFF), (byte)(index >> 8), sub,
|
||||
(byte)(value & 0xFF), (byte)(value >> 8), 0, 0];
|
||||
SendFrame(COB_SDO_TX, d);
|
||||
// Dim($"← SDO RSP 0x{COB_SDO_TX:X3} idx=0x{index:X4} val=0x{value:X4}");
|
||||
}
|
||||
|
||||
private void SendFrame(uint canId, byte[] data)
|
||||
{
|
||||
var bus = _can;
|
||||
if (bus is null || !bus.IsConnected)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
bus.SendFrameAsync(canId, data, _ct).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
private bool TryReceive(out CanFrameReceivedEventArgs frame)
|
||||
{
|
||||
if (_rxQueue.TryDequeue(out frame!))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (!_rxSignal.Wait(10, _ct))
|
||||
{
|
||||
frame = null!;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
frame = null!;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_rxQueue.TryDequeue(out frame!))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
frame = null!;
|
||||
return false;
|
||||
}
|
||||
|
||||
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
|
||||
{
|
||||
_rxQueue.Enqueue(e);
|
||||
try
|
||||
{
|
||||
_rxSignal.Release();
|
||||
}
|
||||
catch (SemaphoreFullException)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// Logging
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
private static string Now => DateTime.Now.ToString("HH:mm:ss.fff");
|
||||
|
||||
private static void Log(string msg)
|
||||
=> Console.WriteLine($"[{Now}] {msg}");
|
||||
|
||||
private static void LogOk(string msg)
|
||||
{
|
||||
Console.ForegroundColor = ConsoleColor.Yellow;
|
||||
Console.WriteLine($"[{Now}] {msg}");
|
||||
Console.ResetColor();
|
||||
}
|
||||
|
||||
private static void LogWarn(string msg)
|
||||
{
|
||||
Console.ForegroundColor = ConsoleColor.Red;
|
||||
Console.WriteLine($"[{Now}] ⚠️ {msg}");
|
||||
Console.ResetColor();
|
||||
}
|
||||
|
||||
private static void Dim(string msg)
|
||||
{
|
||||
Console.ForegroundColor = ConsoleColor.DarkGray;
|
||||
Console.WriteLine($"[{Now}] {msg}");
|
||||
Console.ResetColor();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user