simple-telem-viewer/simple_test.py at main · Solar-Gators/simple-telem-viewer · 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
import serial
import struct
import tkinter as tk

PORT = "COM9"
BAUD = 57600

ser = serial.Serial(PORT, BAUD, timeout=1)
print(f"Listening on {PORT} at {BAUD} baud...")

STX = 0x02
ETX = 0x03
ESC = 0x1B

in_frame = False
escaped = False
frame = bytearray()

# Tkinter setup
root = tk.Tk()
root.title("Telemetry Display")

lat_var = tk.StringVar(value="0.0")
lon_var = tk.StringVar(value="0.0")
speed_var = tk.StringVar(value="0.0 km/h")
sats_var = tk.StringVar(value="0")

tk.Label(root, text="Latitude:").grid(row=0, column=0, sticky="w")
tk.Label(root, textvariable=lat_var).grid(row=0, column=1, sticky="w")

tk.Label(root, text="Longitude:").grid(row=1, column=0, sticky="w")
tk.Label(root, textvariable=lon_var).grid(row=1, column=1, sticky="w")

tk.Label(root, text="Speed:").grid(row=2, column=0, sticky="w")
tk.Label(root, textvariable=speed_var).grid(row=2, column=1, sticky="w")

tk.Label(root, text="Satellites:").grid(row=3, column=0, sticky="w")
tk.Label(root, textvariable=sats_var).grid(row=3, column=1, sticky="w")

# Helper functions
def bytes_to_double(frame_bytes, index=0):
    double_bytes = frame_bytes[index:index + 8]
    if len(double_bytes) != 8:
        raise ValueError("Not enough bytes to convert to double")
    return struct.unpack('<d', double_bytes)[0]

def parse_longitude(frame):
    lon = bytes_to_double(frame, index=2)
    lon_var.set(f"{lon:.6f}")

def parse_latitude(frame):
    lat = bytes_to_double(frame, index=2)
    lat_var.set(f"{lat:.6f}")

def parse_speed_sats(frame):
    if len(frame) >= 8:
        sats = frame[6]
        speed = struct.unpack('<f', frame[2:6])[0]
        speed_var.set(f"{(speed*0.621371):.2f} mph")
        sats_var.set(str(sats))

# Handlers based on frame ID
frame_handlers = {
    b'\x20\x00': parse_longitude,
    b'\x20\x01': parse_latitude,
    b'\x20\x02': parse_speed_sats,
}


def update_serial():
    global in_frame, escaped, frame

    data = ser.read(ser.in_waiting or 1)
    for byte in data:

        if not in_frame:
            if byte == STX:
                frame.clear()
                in_frame = True
            continue

        if escaped:
            frame.append(byte)
            escaped = False
            continue

        if byte == ESC:
            escaped = True
            continue

        if byte == ETX:
            if len(frame) >= 2:
                identifier = bytes(frame[0:2])
                handler = frame_handlers.get(identifier)
                if handler:
                    try:
                        handler(frame)
                    except Exception as e:
                        print(f"Error parsing frame {identifier.hex()}: {e}")
                else:
                    print(f"Unknown frame ID: {identifier.hex()}")
            in_frame = False
            continue

        frame.append(byte)

    # Next update
    root.after(50, update_serial)  # update every 50 ms

# Serial read
root.after(50, update_serial)
root.mainloop()