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

285
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/VartaCanClient.cs Normal file
View File

@@ -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 _)) { }
}
}
}