test_storage_writes.py

#!/usr/bin/env python3
"""Diagnostic: SunSpec scan + FC16 write test for storage control.

Replicates the exact approach used by callifo/fronius_modbus HA component:
1. Scan SunSpec model headers to find actual storage model address
2. Read all 24 storage registers in one block
3. Write using write_registers (FC16) exactly like the HA component
4. Verify by reading back
"""

import inspect
import os
import struct
import sys
import time

from pymodbus.client import ModbusTcpClient

# Load .env file if present
env_path = os.path.join(os.path.dirname(__file__), ".env")
if os.path.isfile(env_path):
    with open(env_path) as f:
        for line in f:
            line = line.strip()
            if line and not line.startswith("#") and "=" in line:
                key, _, val = line.partition("=")
                os.environ.setdefault(key.strip(), val.strip())

from src.config import Config

SUNSPEC_ID_ADDR = 40000
SUNSPEC_FIRST_HEADER = 40002
SUNSPEC_END = 0xFFFF


def main():
    host = Config.MODBUS_HOST
    port = Config.MODBUS_PORT
    unit = Config.MODBUS_UNIT_ID

    client = ModbusTcpClient(host, port=port, timeout=5)

    # Detect pymodbus keywords
    sig = inspect.signature(client.read_holding_registers)
    if "device_id" in sig.parameters:
        unit_kw = {"device_id": unit}
    elif "slave" in sig.parameters:
        unit_kw = {"slave": unit}
    else:
        unit_kw = {"unit": unit}

    sig_w = inspect.signature(client.write_registers)
    if "device_id" in sig_w.parameters:
        write_kw = {"device_id": unit}
    elif "slave" in sig_w.parameters:
        write_kw = {"slave": unit}
    else:
        write_kw = {"unit": unit}

    # Also detect FC6 keyword for comparison
    sig_w6 = inspect.signature(client.write_register)
    if "device_id" in sig_w6.parameters:
        write6_kw = {"device_id": unit}
    elif "slave" in sig_w6.parameters:
        write6_kw = {"slave": unit}
    else:
        write6_kw = {"unit": unit}

    def read(addr, cnt=1):
        return client.read_holding_registers(addr, count=cnt, **unit_kw)

    def write_fc16(addr, value):
        """Write Multiple Registers (function code 16) — what callifo uses."""
        return client.write_registers(addr, values=[value], **write_kw)

    def write_fc6(addr, value):
        """Write Single Register (function code 6) — what we used before."""
        return client.write_register(addr, value, **write6_kw)

    def read_i16(addr):
        r = read(addr, 1)
        if r.isError():
            return None
        return struct.unpack(">h", struct.pack(">H", r.registers[0]))[0]

    def read_u16(addr):
        r = read(addr, 1)
        if r.isError():
            return None
        return r.registers[0]

    print(f"Connecting to {host}:{port} (unit {unit})...")
    if not client.connect():
        print("FAILED")
        sys.exit(1)
    print("Connected.\n")

    # ================================================================
    # STEP 1: SunSpec model scan (like callifo does)
    # ================================================================
    print("=== SUNSPEC MODEL SCAN ===")
    sid = read(SUNSPEC_ID_ADDR, 2)
    if sid.isError():
        print(f"ERROR reading SunSpec ID at {SUNSPEC_ID_ADDR}")
        sys.exit(1)
    if sid.registers[0] != 0x5375 or sid.registers[1] != 0x6E53:
        print(f"Invalid SunSpec ID: {sid.registers} (expected 'SunS')")
        sys.exit(1)
    print(f"SunSpec ID OK at {SUNSPEC_ID_ADDR}")

    addr = SUNSPEC_FIRST_HEADER
    models = {}
    for _ in range(128):
        hdr = read(addr, 2)
        if hdr.isError():
            print(f"  ERROR reading model header at {addr}")
            break
        model_id = hdr.registers[0]
        model_len = hdr.registers[1]
        if model_id == SUNSPEC_END:
            print(f"  End marker at {addr}")
            break
        data_addr = addr + 2
        print(f"  Model {model_id:>4d}  length={model_len:>3d}  header={addr}  data={data_addr}")
        models[model_id] = {"id": model_id, "length": model_len, "header": addr, "data": data_addr}
        addr = data_addr + model_len

    if 124 not in models:
        print("\nStorage model 124 NOT FOUND!")
        client.close()
        sys.exit(1)

    storage = models[124]
    storage_addr = storage["data"]
    our_assumed = 40345  # what our registers.py assumes (pymodbus addr for doc 40346 - 1)
    print(f"\nStorage model 124: data starts at {storage_addr}")
    print(f"Our registers.py assumes: {our_assumed}")
    if storage_addr != our_assumed:
        print(f"*** ADDRESS MISMATCH! Difference = {storage_addr - our_assumed} ***")
    else:
        print("Addresses match.")

    # ================================================================
    # STEP 2: Read all 24 storage registers in one block (like callifo)
    # ================================================================
    print(f"\n=== STORAGE MODEL RAW DUMP (24 regs from {storage_addr}) ===")
    result = read(storage_addr, 24)
    if result.isError():
        print(f"ERROR reading storage block: {result}")
        client.close()
        sys.exit(1)

    regs = result.registers
    labels = [
        "WChaMax", "WChaGra", "WDisChaGra", "StorCtl_Mod", "VAChaMax",
        "MinRsvPct", "ChaState", "StorAval", "InBatV", "ChaSt",
        "OutWRte", "InWRte", "InOutWRte_WinTms", "InOutWRte_RvrtTms", "InOutWRte_RmpTms",
        "ChaGriSet", "WChaMax_SF", "WChaDisChaGra_SF", "VAChaMax_SF", "MinRsvPct_SF",
        "ChaState_SF", "StorAval_SF", "InBatV_SF", "InOutWRte_SF",
    ]
    for i, raw in enumerate(regs):
        label = labels[i] if i < len(labels) else f"reg[{i}]"
        signed = struct.unpack(">h", struct.pack(">H", raw))[0]
        extra = f" (signed: {signed})" if signed != raw else ""
        print(f"  [{i:2d}] {storage_addr+i:>5d}  {label:25s} = {raw:>6d}{extra}")

    # ================================================================
    # STEP 3: Test multiple approaches to force discharge
    # ================================================================
    ctrl_mode_addr = storage_addr + 3   # StorCtl_Mod
    out_w_rte_addr = storage_addr + 10  # OutWRte (discharge)
    in_w_rte_addr = storage_addr + 11   # InWRte (charge)
    rvrt_tms_addr = storage_addr + 13   # InOutWRte_RvrtTms

    status_map = {1: "Off", 2: "Empty", 3: "Discharging", 4: "Charging", 5: "Full", 6: "Holding", 7: "Testing"}

    def read_status():
        r = read(storage_addr, 24)
        if r.isError():
            return None
        regs = r.registers
        ctrl = regs[3]
        soc = struct.unpack(">h", struct.pack(">H", regs[6]))[0] * 0.01
        chst = regs[9]
        out = struct.unpack(">h", struct.pack(">H", regs[10]))[0]
        inr = struct.unpack(">h", struct.pack(">H", regs[11]))[0]
        mode_map = {0: "Auto", 1: "Charge", 2: "Discharge", 3: "Charge+Discharge"}
        print(f"    ctrl={ctrl}({mode_map.get(ctrl,'?')}), ChaSt={chst}({status_map.get(chst,'?')}), "
              f"SoC={soc:.1f}%, OutWRte={out}, InWRte={inr}")
        return chst

    def revert():
        write_fc16(ctrl_mode_addr, 0)
        write_fc16(out_w_rte_addr, 10000)
        write_fc16(in_w_rte_addr, 10000)
        write_fc16(rvrt_tms_addr, 0)

    def test_approach(name, mode, out_rte, in_rte):
        print(f"\n--- {name} ---")
        print(f"  Writing: StorCtl_Mod={mode}, OutWRte={out_rte}, InWRte={in_rte}")
        revert()  # clean slate
        time.sleep(1)
        write_fc16(ctrl_mode_addr, mode)
        write_fc16(out_w_rte_addr, out_rte)
        write_fc16(in_w_rte_addr, in_rte)
        print(f"  After write:")
        read_status()
        print(f"  Waiting 10s...")
        time.sleep(10)
        print(f"  After 10s:")
        chst = read_status()
        return chst

    print(f"\n=== TESTING MULTIPLE APPROACHES ===")
    print(f"  Addrs: ctrl={ctrl_mode_addr}, out={out_w_rte_addr}, in={in_w_rte_addr}")

    # Approach A: mode=2 (standard SunSpec discharge)
    test_approach("A: mode=2, both rates 100%",
                  mode=2, out_rte=10000, in_rte=10000)

    # Approach B: mode=3 + block charging (callifo "Block Charging" mode)
    # Issue #67 confirms mode=3 works on FW 1.38.7-1
    test_approach("B: mode=3, OutWRte=100%, InWRte=0% (block charging)",
                  mode=3, out_rte=10000, in_rte=0)

    # Approach C: mode=1 with InWRte=0 (callifo "Discharge to Grid" mode)
    test_approach("C: mode=1, OutWRte=100%, InWRte=0% (discharge to grid)",
                  mode=1, out_rte=10000, in_rte=0)

    # Approach D: mode=2 with negative InWRte (force grid discharge)
    # Negative values use two's complement: -10000 = 65536 - 10000 = 55536
    test_approach("D: mode=2, OutWRte=100%, InWRte=-100% (negative = grid feed)",
                  mode=2, out_rte=10000, in_rte=55536)

    # Approach E: mode=1 with negative InWRte
    test_approach("E: mode=1, OutWRte=100%, InWRte=-100% (negative charge = discharge)",
                  mode=1, out_rte=10000, in_rte=55536)

    # ================================================================
    # STEP 4: Revert to auto
    # ================================================================
    print(f"\n=== REVERTING TO AUTO ===")
    revert()
    ctrl_rev = read_u16(ctrl_mode_addr)
    print(f"  Reverted: ctrl_mode={ctrl_rev}")

    client.close()
    print("\nDone.")


if __name__ == "__main__":
    main()