ModbusLatencyLogger

#!/usr/bin/env python3
import socket
import struct
import time
import csv
from datetime import datetime

ip = "192.168.1.7"
port = 502

unit_id = 1
start_reg = 4607      # 304608
count = 20            # number of registers to read
interval_s = 0.001      # 0.1 = 100 ms between reads
timeout_s = 2.0
csv_file = "modbus_latency_log.csv"


def build_modbus_read_request(transaction_id, unit_id, start_reg, count):
    protocol_id = 0
    length = 6
    function_code = 4  # Read Input Registers
    return struct.pack(
        ">HHHBBHH",
        transaction_id,
        protocol_id,
        length,
        unit_id,
        function_code,
        start_reg,
        count,
    )


def recv_exact(sock, n):
    data = b""
    while len(data) < n:
        chunk = sock.recv(n - len(data))
        if not chunk:
            raise ConnectionError("Socket closed by peer")
        data += chunk
    return data


def read_modbus_once(sock, transaction_id, unit_id, start_reg, count):
    req = build_modbus_read_request(transaction_id, unit_id, start_reg, count)

    t0 = time.perf_counter()
    sock.sendall(req)

    # Read MBAP header first
    header = recv_exact(sock, 6)
    rx_tid, rx_pid, rx_len = struct.unpack(">HHH", header)

    # Then read remaining bytes
    body = recv_exact(sock, rx_len)
    t1 = time.perf_counter()

    resp = header + body
    latency_ms = (t1 - t0) * 1000.0

    unit = body[0]
    func = body[1]

    status = "ok"
    exception_code = ""
    byte_count = ""
    register_values = []

    if func == 0x84:
        status = "modbus_exception"
        exception_code = body[2]
    elif func != 0x04:
        status = f"unexpected_function_0x{func:02X}"
    else:
        byte_count = body[2]
        data = body[3:3 + byte_count]
        register_values = [
            struct.unpack(">H", data[i:i+2])[0]
            for i in range(0, len(data), 2)
        ]

    return {
        "latency_ms": latency_ms,
        "status": status,
        "exception_code": exception_code,
        "transaction_id": rx_tid,
        "unit_id": unit,
        "function_code": func,
        "byte_count": byte_count,
        "registers": register_values,
        #"raw_response_hex": resp.hex(),
    }


def main():
    fieldnames = [
        "timestamp_utc",
        "elapsed_s",
        "sample",
        "latency_ms",
        "status",
        "exception_code",
        "transaction_id",
        "unit_id",
        "function_code",
        "byte_count",
        #"raw_response_hex",
    ]

    start_time = time.time()
    sample = 0
    transaction_id = 1

    print(f"Logging to {csv_file}")
    print("Press Ctrl+C to stop.\n")

    with open(csv_file, "w", newline="") as f:
        writer = csv.DictWriter(f, fieldnames=fieldnames)
        writer.writeheader()

        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.settimeout(timeout_s)
            s.connect((ip, port))

            while True:
                sample += 1
                now = datetime.utcnow().isoformat() + "Z"
                elapsed_s = time.time() - start_time

                try:
                    result = read_modbus_once(
                        s, transaction_id, unit_id, start_reg, count
                    )

                    row = {
                        "timestamp_utc": now,
                        "elapsed_s": f"{elapsed_s:.3f}",
                        "sample": sample,
                        "latency_ms": f"{result['latency_ms']:.3f}",
                        "status": result["status"],
                        "exception_code": result["exception_code"],
                        "transaction_id": result["transaction_id"],
                        "unit_id": result["unit_id"],
                        "function_code": f"0x{result['function_code']:02X}",
                        "byte_count": result["byte_count"],
                        #"raw_response_hex": result["raw_response_hex"],
                    }
                    writer.writerow(row)
                    f.flush()

                    print(
                        f"[{sample:06d}] "
                        f"{row['elapsed_s']} s  "
                        f"{row['latency_ms']} ms  "
                        f"{row['status']}"
                    )

                except socket.timeout:
                    row = {
                        "timestamp_utc": now,
                        "elapsed_s": f"{elapsed_s:.3f}",
                        "sample": sample,
                        "latency_ms": "",
                        "status": "timeout",
                        "exception_code": "",
                        "transaction_id": transaction_id,
                        "unit_id": unit_id,
                        "function_code": "0x04",
                        "byte_count": "",
                        #"raw_response_hex": "",
                    }
                    writer.writerow(row)
                    f.flush()
                    print(f"[{sample:06d}] {row['elapsed_s']} s  TIMEOUT")

                except Exception as e:
                    row = {
                        "timestamp_utc": now,
                        "elapsed_s": f"{elapsed_s:.3f}",
                        "sample": sample,
                        "latency_ms": "",
                        "status": f"error: {e}",
                        "exception_code": "",
                        "transaction_id": transaction_id,
                        "unit_id": unit_id,
                        "function_code": "0x04",
                        "byte_count": "",
                        #"raw_response_hex": "",
                    }
                    writer.writerow(row)
                    f.flush()
                    print(f"[{sample:06d}] {row['elapsed_s']} s  ERROR: {e}")
                    break

                transaction_id = (transaction_id + 1) & 0xFFFF
                if transaction_id == 0:
                    transaction_id = 1

                time.sleep(interval_s)


if __name__ == "__main__":
    main()