Innovenergy_trunk/csharp/Lib/Devices/Battery48TL/ModbusParser.cs

274 lines
11 KiB
C#

using System.Diagnostics.CodeAnalysis;
using System.Text;
using InnovEnergy.Lib.Protocols.Modbus.Conversions;
using InnovEnergy.Lib.Units.Composite;
using InnovEnergy.Lib.Utils;
using static InnovEnergy.Lib.Devices.Battery48TL.LedState;
namespace InnovEnergy.Lib.Devices.Battery48TL;
public static class ModbusParser
{
internal static Battery48TLStatus ParseBatteryStatus(this ModbusRegisters data)
{
var greenLed = data.ParseLedState(register: 1005, led: LedColor.Green);
var amberLed = data.ParseLedState(register: 1005, led: LedColor.Amber);
var blueLed = data.ParseLedState(register: 1005, led: LedColor.Blue);
var redLed = data.ParseLedState(register: 1005, led: LedColor.Red);
var soc = data.ParseSoc();
// var eoc = greenLed is On
// && amberLed is Off
// && blueLed is Off;
var eoc = data.ParseEocReached();
var maxSoc = eoc ? 100m : 99.9m;
var batteryCold = greenLed >= BlinkingSlow
&& blueLed >= BlinkingSlow;
var temperatureState = batteryCold
? TemperatureState.Cold
: TemperatureState.OperatingTemperature; // TODO: overheated
return new Battery48TLStatus
{
Dc = data.ParseDcBus(),
Alarms = data.ParseAlarms().ToList(),
Warnings = data.ParseWarnings().ToList(),
Soc = Math.Min(soc, maxSoc),
Temperature = data.ParseDecimal(register: 1004, scaleFactor: 0.1m, offset: -400),
GreenLed = greenLed,
AmberLed = amberLed,
BlueLed = blueLed,
RedLed = redLed,
Heating = data.ParseBool(baseRegister: 1014, bit: 6),
ConnectedToDc = data.ParseBool(baseRegister: 1014, bit: 0),
TemperatureState = temperatureState,
MaxChargingPower = data.CalcMaxChargePower(),
MaxDischargingPower = data.CalcMaxDischargePower(),
CellsVoltage = data.ParseDecimal(register: 1000, scaleFactor: 0.01m),
TotalCurrent = data.ReadTotalCurrent(),
EocReached = eoc
};
}
public static Decimal ParseDecimal(this ModbusRegisters data, Int32 register, Decimal scaleFactor = 1.0m, Double offset = 0.0)
{
var value = data[register].ConvertTo<Int32>(); // widen to 32bit signed
if (value >= 0x8000)
value -= 0x10000; // Fiamm stores their integers signed AND with sign-offset @#%^&!
return (Decimal)(value + offset) * scaleFactor;
}
internal static Decimal ParseCurrent(this ModbusRegisters data)
{
return data.ParseDecimal(register: 1001, scaleFactor: 0.01m, offset: -10000);
}
internal static Decimal ParseBusVoltage(this ModbusRegisters data)
{
return data.ParseDecimal(register: 1002, scaleFactor: 0.01m);
}
internal static Decimal ReadTotalCurrent(this ModbusRegisters data)
{
try
{
return ParseDecimal(data, register: 1063, scaleFactor: 0.01m, offset: -100);
}
catch (Exception e)
{
Console.WriteLine(e + " Read Total current fail ");
throw;
}
}
internal static Boolean ParseBool(this ModbusRegisters data, Int32 baseRegister, Int16 bit)
{
var x = bit / 16;
var y = bit % 16;
var value = (UInt32)data[baseRegister + x];
return (value & (1 << y)) > 0;
}
internal static LedState ParseLedState(this ModbusRegisters data, Int32 register, LedColor led)
{
var lo = data.ParseBool(register, (led.ConvertTo<Int16>() * 2 ).ConvertTo<Int16>());
var hi = data.ParseBool(register, (led.ConvertTo<Int16>() * 2 + 1).ConvertTo<Int16>());
return (hi, lo) switch
{
(false, false) => Off,
(false, true) => On,
(true, false) => BlinkingSlow,
(true, true) => BlinkingFast,
};
}
internal static String ParseString(this ModbusRegisters data, Int32 register, Int16 count)
{
return Enumerable
.Range(register, count)
.Select(i => data[i])
.Select(BitConverter.GetBytes)
.Select(Encoding.ASCII.GetString)
.Aggregate("", (a, b) => a + b[1] + b[0]); // endian swap
}
internal static Boolean ParseEocReached(this ModbusRegisters data)
{
var s = ParseString(data, 1061, 2);
return "EOC_" == s;
}
internal static Decimal ParseSoc(this ModbusRegisters data)
{
return data.ParseDecimal(register: 1054, scaleFactor: 0.1m);
}
private static Decimal CalcPowerLimitImposedByVoltageLimit(Decimal v,Decimal i,Decimal vLimit,Decimal rInt)
{
var dv = vLimit - v;
var di = dv / rInt;
var pLimit = vLimit * (i + di);
return pLimit;
}
private static Decimal CalcPowerLimitImposedByCurrentLimit(Decimal v, Decimal i, Decimal iLimit, Decimal rInt)
{
var di = iLimit - i;
var dv = di * rInt;
var pLimit = iLimit * (v + dv);
return pLimit;
}
private static Decimal CalcPowerLimitImposedByTempLimit(Decimal t, Decimal maxAllowedTemp, Decimal power , Decimal setpoint)
{
// const Int32 holdZone = 300;
// const Int32 maxAllowedTemp = 315;
var kp = 0.05m;
var error = setpoint - power;
var controlOutput = (kp * error) *(1 - Math.Abs((t-307.5m)/7.5m));
return controlOutput;
// var a = holdZone - maxAllowedTemp;
// var b = -a * maxAllowedTemp;
}
internal static Decimal CalcMaxChargePower(this ModbusRegisters data)
{
var v = data.ParseDecimal(register: 1000, scaleFactor: 0.01m);
var i = ParseCurrent(data);
var pLimits = new[]
{
// TODO: review
CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMax, Constants.RIntMin),
CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMax, Constants.RIntMax),
CalcPowerLimitImposedByCurrentLimit(v, i, Constants.IMax, Constants.RIntMin),
CalcPowerLimitImposedByCurrentLimit(v, i, Constants.IMax, Constants.RIntMax)
};
var pLimit = pLimits.Min();
return Math.Max(pLimit, 0);
}
internal static DcBus ParseDcBus(this ModbusRegisters data) => new()
{
Current = data.ParseCurrent(),
Voltage = data.ParseBusVoltage(),
};
internal static Decimal CalcMaxDischargePower(this ModbusRegisters data)
{
var v = data.ParseDecimal(register: 1000, scaleFactor: 0.01m);
var i = ParseCurrent(data);
var pLimits = new[]
{
// TODO: review
CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMin, Constants.RIntMin),
CalcPowerLimitImposedByVoltageLimit(v, i, Constants.VMin, Constants.RIntMax),
CalcPowerLimitImposedByCurrentLimit(v, i, -Constants.IMax, Constants.RIntMin),
CalcPowerLimitImposedByCurrentLimit(v, i, -Constants.IMax, Constants.RIntMax),
// CalcPowerLimitImposedByTempLimit(t,315,300)
};
var pLimit = pLimits.Max();
return Math.Min(pLimit, 0);
}
[SuppressMessage("ReSharper", "StringLiteralTypo")]
internal static IEnumerable<String> ParseAlarms(this ModbusRegisters data)
{
if (data.ParseBool(1010, 0)) yield return "Tam : BMS temperature too low";
if (data.ParseBool(1010, 2)) yield return "TaM2 : BMS temperature too high";
if (data.ParseBool(1010, 3)) yield return "Tbm : Battery temperature too low";
if (data.ParseBool(1010, 5)) yield return "TbM2 : Battery temperature too high";
if (data.ParseBool(1010, 7)) yield return "VBm2 : Bus voltage too low";
if (data.ParseBool(1010, 9)) yield return "VBM2 : Bus voltage too high";
if (data.ParseBool(1010, 11)) yield return "IDM2 : Discharge current too high";
if (data.ParseBool(1010, 12)) yield return "ISOB : Electrical insulation failure";
if (data.ParseBool(1010, 13)) yield return "MSWE : Main switch failure";
if (data.ParseBool(1010, 14)) yield return "FUSE : Main fuse blown";
if (data.ParseBool(1010, 15)) yield return "HTRE : Battery failed to warm up";
if (data.ParseBool(1010, 16)) yield return "TCPE : Temperature sensor failure";
if (data.ParseBool(1010, 17)) yield return "STRE :";
if (data.ParseBool(1010, 18)) yield return "CME : Current sensor failure";
if (data.ParseBool(1010, 19)) yield return "HWFL : BMS hardware failure";
if (data.ParseBool(1010, 20)) yield return "HWEM : Hardware protection tripped";
if (data.ParseBool(1010, 21)) yield return "ThM : Heatsink temperature too high";
if (data.ParseBool(1010, 22)) yield return "vsm1 : String voltage too low";
if (data.ParseBool(1010, 23)) yield return "vsm2 : Low string voltage failure";
if (data.ParseBool(1010, 25)) yield return "vsM2 : String voltage too high";
if (data.ParseBool(1010, 27)) yield return "iCM2 : Charge current too high";
if (data.ParseBool(1010, 29)) yield return "iDM2 : Discharge current too high";
if (data.ParseBool(1010, 31)) yield return "MID2 : String voltage unbalance too high";
if (data.ParseBool(1010, 33)) yield return "CCBF : Internal charger hardware failure";
if (data.ParseBool(1010, 34)) yield return "AhFL :";
if (data.ParseBool(1010, 36)) yield return "TbCM :";
if (data.ParseBool(1010, 37)) yield return "BRNF :";
if (data.ParseBool(1010, 42)) yield return "HTFS : If Heaters Fuse Blown";
if (data.ParseBool(1010, 43)) yield return "DATA : Parameters out of range";
if (data.ParseBool(1010, 45)) yield return "CELL2:";
}
[SuppressMessage("ReSharper", "StringLiteralTypo")]
internal static IEnumerable<String> ParseWarnings(this ModbusRegisters data)
{
if (data.ParseBool(1006, 1)) yield return "TaM1: BMS temperature high";
if (data.ParseBool(1006, 4)) yield return "TbM1: Battery temperature high";
if (data.ParseBool(1006, 6)) yield return "VBm1: Bus voltage low";
if (data.ParseBool(1006, 8)) yield return "VBM1: Bus voltage high";
if (data.ParseBool(1006, 10)) yield return "IDM1: Discharge current high";
if (data.ParseBool(1006, 24)) yield return "vsM1: String voltage high";
if (data.ParseBool(1006, 26)) yield return "iCM1: Charge current high";
if (data.ParseBool(1006, 28)) yield return "iDM1: Discharge current high";
if (data.ParseBool(1006, 30)) yield return "MID1: String voltages unbalanced";
if (data.ParseBool(1006, 32)) yield return "BLPW: Not enough charging power on bus";
if (data.ParseBool(1006, 35)) yield return "Ah_W: String SOC low";
if (data.ParseBool(1006, 38)) yield return "MPMM: Midpoint wiring problem";
if (data.ParseBool(1006, 39)) yield return "TCMM:";
if (data.ParseBool(1006, 40)) yield return "TCdi: Temperature difference between strings high";
if (data.ParseBool(1006, 41)) yield return "WMTO:";
if (data.ParseBool(1006, 44)) yield return "bit44:";
if (data.ParseBool(1006, 46)) yield return "CELL1:";
}
}