# OUTPUT_MODE=LIVE

import serial
import time
import struct

PORT = "/dev/ttyACM1"
BAUD = 115200

MSP_RAW_GPS = 106


def msp_request(s, cmd):
    size = 0
    checksum = size ^ cmd

    packet = b"$M<" + bytes([size, cmd, checksum])

    s.reset_input_buffer()
    s.write(packet)
    s.flush()

    start = time.time()

    while time.time() - start < 0.3:

        if s.read(1) != b"$":
            continue

        if s.read(1) != b"M":
            continue

        direction = s.read(1)

        if direction not in (b">", b"!"):
            continue

        size_b = s.read(1)
        cmd_b = s.read(1)

        if not size_b or not cmd_b:
            continue

        size = size_b[0]
        response_cmd = cmd_b[0]

        data = s.read(size)
        checksum_rx = s.read(1)

        if response_cmd != cmd:
            continue

        if direction == b"!":
            return None

        checksum = size ^ response_cmd

        for b in data:
            checksum ^= b

        if not checksum_rx or checksum != checksum_rx[0]:
            return None

        return data

    return None


print("Подключение к полётнику...", flush=True)

s = serial.Serial(
    PORT,
    BAUD,
    timeout=0.05
)

time.sleep(1)

print("Читаю GPS через MSP...", flush=True)


while True:

    data = msp_request(s, MSP_RAW_GPS)

    if data is not None and len(data) >= 16:

        fix = data[0]
        sats = data[1]

        lat = struct.unpack(
            "<i",
            data[2:6]
        )[0] / 10000000.0

        lon = struct.unpack(
            "<i",
            data[6:10]
        )[0] / 10000000.0

        alt = struct.unpack(
            "<H",
            data[10:12]
        )[0]

        speed = struct.unpack(
            "<H",
            data[12:14]
        )[0] / 100.0

        course = struct.unpack(
            "<H",
            data[14:16]
        )[0] / 10.0

        print(
            "GPS: %.7f, %.7f | Высота: %d м | Спутники: %d | Fix: %d | Скорость: %.2f м/с | Курс: %.1f°"
            % (
                lat,
                lon,
                alt,
                sats,
                fix,
                speed,
                course
            ),
            flush=True
        )

    else:
        print("GPS: нет данных", flush=True)

    time.sleep(0.2)