dma-03-python/test_packet_parser.py at main · tecgihan/dma-03-python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
"""パケット解析テスト（ハードウェア不要）。

合成したバイト列を使って、符号付き int16 の復元・ヘッダ／フッタ検証・
ストリーム同期（先頭のゴミ除去）・有効ch数による可変長が正しく動くことを
確認する。

使い方::

    python test_packet_parser.py
"""

from __future__ import annotations

import struct

import numpy as np

import packet_parser as pp


def build_packet(samples: list[list[int]], enabled_ch_count: int) -> bytes:
    """``samples`` (各要素 enabled_ch_count ch) からパケットバイト列を組み立てる。"""
    fmt = f"<{enabled_ch_count}h"
    body = b"".join(struct.pack(fmt, *s) for s in samples)
    return pp.HEADER + body + pp.FOOTER


def test_parse_now_signed() -> None:
    raw = struct.pack("<3h", -32768, 0, 32767)
    arr = pp.parse_now(raw, 3)
    assert arr.dtype == np.int16
    assert list(arr) == [-32768, 0, 32767]
    print("  parse_now (3ch): OK")


def test_parse_now_variable_length() -> None:
    # 有効ch=2 のとき 4 バイト
    raw = struct.pack("<2h", 100, -100)
    arr = pp.parse_now(raw, 2)
    assert list(arr) == [100, -100]
    print("  parse_now (2ch 可変長): OK")


def test_parse_packet_shape_and_values() -> None:
    matome, enabled = 3, 3
    samples = [
        [1, 2, 3],
        [-1, -2, -3],
        [32767, -32768, 0],
    ]
    raw = build_packet(samples, enabled)
    assert len(raw) == pp.packet_size(matome, enabled)
    data = pp.parse_packet(raw, matome, enabled)
    assert data.shape == (matome, enabled)
    assert np.array_equal(data, np.array(samples, dtype=np.int16))
    print("  parse_packet (3ch): OK")


def test_parse_packet_enabled_subset() -> None:
    # 2ch だけ有効（例: CH1, CH3 のみ有効 → 列は 2 本）
    matome, enabled = 2, 2
    samples = [[111, 222], [-333, 444]]
    raw = build_packet(samples, enabled)
    assert len(raw) == pp.packet_size(matome, enabled)
    data = pp.parse_packet(raw, matome, enabled)
    assert data.shape == (matome, enabled)
    assert np.array_equal(data, np.array(samples, dtype=np.int16))
    print("  parse_packet (2ch 有効): OK")


def test_bad_header_footer() -> None:
    matome, enabled = 2, 3
    raw = bytearray(build_packet([[0] * 3, [0] * 3], enabled))
    bad = bytes([0x00]) + bytes(raw[1:])
    try:
        pp.parse_packet(bad, matome, enabled)
    except pp.PacketError:
        print("  bad header detected: OK")
    else:
        raise AssertionError("ヘッダ不正を検出できませんでした")
    bad = bytes(raw[:-1]) + bytes([0x00])
    try:
        pp.parse_packet(bad, matome, enabled)
    except pp.PacketError:
        print("  bad footer detected: OK")
    else:
        raise AssertionError("フッタ不正を検出できませんでした")


def test_find_packet_with_garbage() -> None:
    matome, enabled = 2, 3
    pkt = build_packet([[7, 8, 9], [13, 14, 15]], enabled)
    stream = b"\x12\x34garbage" + pkt + b"\xAA\xAA"  # 末尾に途中ヘッダ
    data, consumed = pp.find_packet(stream, matome, enabled)
    assert data is not None, "パケットを見つけられませんでした"
    assert np.array_equal(data[0], np.array([7, 8, 9], dtype=np.int16))
    rest = stream[consumed:]
    assert rest == b"\xAA\xAA"
    print("  find_packet (sync over garbage): OK")


def test_parse_robot_hex() -> None:
    # 0001=1, FFFF=-1, 7FFF=32767（3ch）
    line = "0001FFFF7FFF"
    arr = pp.parse_robot_hex(line, 3)
    assert arr.dtype == np.int16
    assert list(arr) == [1, -1, 32767], list(arr)
    # 2ch 有効のとき
    arr2 = pp.parse_robot_hex("00648000", 2)
    assert list(arr2) == [100, -32768], list(arr2)
    print("  parse_robot_hex (可変長): OK")


def test_ad_to_eng() -> None:
    ad = np.array([[32000, -32000, 16000]], dtype=np.int16)
    fs = [5000, 5000, 5000]
    eng = pp.ad_to_eng(ad, fs)
    assert np.isclose(eng[0, 0], 5000.0)     # 満点
    assert np.isclose(eng[0, 1], -5000.0)
    assert np.isclose(eng[0, 2], 2500.0)     # 16000/32000*5000
    print("  ad_to_eng: OK")


def main() -> int:
    print("== packet_parser テスト ==")
    test_parse_now_signed()
    test_parse_now_variable_length()
    test_parse_packet_shape_and_values()
    test_parse_packet_enabled_subset()
    test_bad_header_footer()
    test_find_packet_with_garbage()
    test_parse_robot_hex()
    test_ad_to_eng()
    print("すべて成功しました。")
    return 0


if __name__ == "__main__":
    raise SystemExit(main())