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

using System.Diagnostics.CodeAnalysis;
using InnovEnergy.Lib.Protocols.Modbus.Conversions;
using InnovEnergy.Lib.Units;
using InnovEnergy.Lib.Units.Composite;
using InnovEnergy.Lib.Utils;

namespace InnovEnergy.Lib.Devices.Battery48TL;

public static class ModbusParser
{
    internal static Battery48TLStatus ParseBatteryStatus(this ModbusRegisters data)
    {
        return new Battery48TLStatus
        {
            Dc                  = data.ParseDcBus(),
            Alarms              = data.ParseAlarms().ToList(),
            Warnings            = data.ParseWarnings().ToList(),
            Soc                 = data.ParseSoc(),
            Temperature         = data.ParseTemperature(),
            GreenLed            = data.ParseGreenLed(),
            AmberLed            = data.ParseAmberLed(),
            BlueLed             = data.ParseBlueLed(),
            RedLed              = data.ParseRedLed(),
            MainSwitchState     = data.ParseMainSwitchState(),
            HeaterState         = data.ParseHeaterState(),
            EocState            = data.ParseEocState(),
            TemperatureState    = data.ParseTemperatureState(), 
            MaxChargingPower    = data.CalcMaxChargePower(),
            MaxDischargingPower = data.CalcMaxDischargePower(),
            CellsVoltage        = data.ParseCellsVoltage(),
        };
    }
    
    
    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 ParseCellsVoltage(this ModbusRegisters data)
    {
        return data.ParseDecimal(register:  1000, scaleFactor: 0.01m);
    }
    
    internal static Decimal ParseBusVoltage(this ModbusRegisters data)
    {
        return data.ParseDecimal(register: 1002, scaleFactor: 0.01m);
    }
    
    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) => LedState.Off,
            (false, true)  => LedState.On,
            (true, false)  => LedState.BlinkingSlow,
            (true, true)   => LedState.BlinkingFast,
        };
    }


    private static Boolean ParseEocReached(this ModbusRegisters data)
    {
        return ParseLedState(data, 1005, LedColor.Green) == LedState.On &&
               ParseLedState(data, 1005, LedColor.Amber) == LedState.Off &&
               ParseLedState(data, 1005, LedColor.Blue) == LedState.Off;
    }
    
    internal static State ParseTemperatureState(this ModbusRegisters data)
    {
        return data.ParseBatteryCold() ? "cold" : "operating temperature";  // TODO: overheated,
    }
    
    internal static Decimal ParseTemperature(this ModbusRegisters data)
    {
        return data.ParseDecimal(register: 1004, scaleFactor: 0.1m, offset: -400);
    }

    internal static Decimal ParseSoc(this ModbusRegisters data)
    {
        return data.ParseDecimal(register: 1054, scaleFactor: 0.1m);
    }

    internal static State ParseEocState(this ModbusRegisters data)
    {
        return data.ParseEocReached() ? "EOC reached" : "EOC not reached";
    }
    
    internal static State ParseHeaterState(this ModbusRegisters data)
    {
        return data.ParseBool(baseRegister: 1014, bit: 6) ? "heating" : "not heating";
    }

    internal static State ParseMainSwitchState(this ModbusRegisters data)
    {
        return data.ParseBool(baseRegister: 1014, bit: 0) ? "connected to bus" : "disconnected from bus";
    }

    internal static Boolean ParseBatteryCold(this ModbusRegisters data)
    {
        return ParseLedState(data, 1005, LedColor.Green) >= LedState.BlinkingSlow &&
               ParseLedState(data, 1005, LedColor.Blue) >= LedState.BlinkingSlow;
    }

    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 = ParseCellsVoltage(data);
        var i = ParseCurrent(data);

        var pLimits = new[]
        {
            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)
    {
        return new()
        {
            Current = data.ParseCurrent(),
            Voltage = data.ParseBusVoltage(),
        };
    }

    internal static Decimal CalcMaxDischargePower(this ModbusRegisters data)
    {
        var v = ParseCellsVoltage(data);
        var i = ParseCurrent(data);
        var t = data.ParseDecimal(register: 1004, scaleFactor: 0.1m, offset: -400);

        var pLimits = new[]
        {
            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);
    }


    internal static LedState ParseGreenLed(this ModbusRegisters data) => data.ParseLedState(register: 1005, led: LedColor.Green);
    internal static LedState ParseAmberLed(this ModbusRegisters data) => data.ParseLedState(register: 1006, led: LedColor.Amber);
    internal static LedState ParseBlueLed (this ModbusRegisters data) => data.ParseLedState(register: 1005, led: LedColor.Blue);
    internal static LedState ParseRedLed  (this ModbusRegisters data) => data.ParseLedState(register: 1005, led: LedColor.Red);
    
    
    [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:";
    }
}