# OUTPUT_MODE=LIVE

import serial
import time
import struct

PORT = "/dev/ttyACM0"
BAUD = 115200

# MSP v1 команды
MSP_RAW_IMU  = 102
MSP_RAW_GPS  = 106
MSP_ATTITUDE = 108
MSP_ALTITUDE = 109
MSP_ANALOG   = 110


def msp_request(s, cmd):
    # MSP v1: $M< + size + cmd + checksum
    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:

        c = s.read(1)

        if c != 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:
            return None

        if 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("Полетник подключён. Читаю датчики...", flush=True)


while True:

    parts = []

    # -------------------------------------------------
    # IMU
    # -------------------------------------------------

    data = msp_request(s, MSP_RAW_IMU)

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

        ax, ay, az, gx, gy, gz, mx, my, mz = struct.unpack(
            "<9h",
            data[:18]
        )

        parts.append(
            "ACC: X=%d Y=%d Z=%d | "
            "GYRO: X=%d Y=%d Z=%d | "
            "MAG: X=%d Y=%d Z=%d"
            % (
                ax, ay, az,
                gx, gy, gz,
                mx, my, mz
            )
        )

    else:
        parts.append("IMU: нет данных")


    # -------------------------------------------------
    # Ориентация
    # -------------------------------------------------

    data = msp_request(s, MSP_ATTITUDE)

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

        roll, pitch, yaw = struct.unpack(
            "<hhh",
            data[:6]
        )

        roll /= 10.0
        pitch /= 10.0

        parts.append(
            "Roll: %.1f° | Pitch: %.1f° | Yaw: %d°"
            % (
                roll,
                pitch,
                yaw
            )
        )

    else:
        parts.append("ATT: нет данных")


    # -------------------------------------------------
    # Высота / барометр
    # -------------------------------------------------

    data = msp_request(s, MSP_ALTITUDE)

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

        altitude_cm = struct.unpack(
            "<i",
            data[:4]
        )[0]

        vario = struct.unpack(
            "<h",
            data[4:6]
        )[0]

        altitude = altitude_cm / 100.0

        parts.append(
            "Высота: %.2f м | Vario: %.2f м/с"
            % (
                altitude,
                vario / 100.0
            )
        )

    else:
        parts.append("Высота: нет данных")


    # -------------------------------------------------
    # GPS
    # -------------------------------------------------

    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]

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

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

    else:
        parts.append("GPS: нет данных")


    # -------------------------------------------------
    # Питание
    # -------------------------------------------------

    data = msp_request(s, MSP_ANALOG)

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

        voltage = data[0] / 10.0

        parts.append(
            "BAT: %.1f V"
            % voltage
        )

    else:
        parts.append("BAT: нет данных")


    # -------------------------------------------------
    # Одна строка для нашего поля Результат
    # -------------------------------------------------

    print(
        " || ".join(parts),
        flush=True
    )

    time.sleep(0.2)