GUI-ROV/rov_axes.py at main · Customize5773/GUI-ROV · 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
"""Pemetaan MURNI axis GUI -> field MANUAL_CONTROL MAVLink.

Dipisah dari rov_agent.py supaya logika clamp/konversi bisa di-unit-test tanpa
bergantung pada pymavlink, socket, atau hardware.

Kenapa MANUAL_CONTROL (bukan RC_CHANNELS_OVERRIDE)?
    MANUAL_CONTROL adalah cara standar ArduSub menerima kontrol manual dari
    ground station: empat sumbu (x=maju/mundur, y=lateral, z=throttle, r=yaw)
    plus bitmask tombol. Ini tidak menimpa channel RC fisik dan lebih aman
    dipakai berdampingan dengan konfigurasi channel/servo di sisi Pixhawk.

Konvensi:
    GUI  : keempat axis -1000..1000, 0 = diam (lihat clampAxis di server.js).
    Wire : x, y, r -> -1000..1000 (netral 0); z -> 0..1000 (netral 500).
"""

# Rentang valid per axis di sisi GUI/UDP.
AXIS_RANGE = {
    "surge": (-1000, 1000),
    "sway": (-1000, 1000),
    "yaw": (-1000, 1000),
    "heave": (-1000, 1000),
}

# Nilai "diam" per axis dalam konvensi GUI.
AXIS_NEUTRAL = {"surge": 0, "sway": 0, "yaw": 0, "heave": 0}

# Netral throttle MANUAL_CONTROL.z di ArduSub.
Z_NEUTRAL = 500


def clamp_axis(name, value):
    """Clamp nilai axis ke rentang valid. Nilai tidak valid -> 0."""
    lo, hi = AXIS_RANGE.get(name, (-1000, 1000))
    try:
        v = int(round(float(value)))
    except (TypeError, ValueError):
        return 0
    return max(lo, min(hi, v))


def to_mavlink_z(heave):
    """Ubah heave GUI (-1000..1000, 0 = diam) ke MANUAL_CONTROL.z ArduSub
    (0..1000, 500 = diam)."""
    return max(0, min(1000, int(round(Z_NEUTRAL + clamp_axis("heave", heave) / 2.0))))


def axes_to_manual_control(surge=0, sway=0, yaw=0, heave=0, buttons=0):
    """Terjemahkan axis GUI ke field MANUAL_CONTROL.

    surge -> x (maju/mundur)
    sway  -> y (lateral kiri/kanan)
    heave -> z (throttle naik/turun)
    yaw   -> r (rotasi)
    buttons: bitmask tombol (placeholder; belum dipetakan ke tombol GUI).
    """
    return {
        "x": clamp_axis("surge", surge),
        "y": clamp_axis("sway", sway),
        "z": to_mavlink_z(heave),
        "r": clamp_axis("yaw", yaw),
        "buttons": int(buttons) & 0xFFFF,
    }


# Perintah netral / fail-safe: diam di tempat, throttle di tengah.
NEUTRAL = axes_to_manual_control(**AXIS_NEUTRAL)

# Tidak ada axis baru dari GUI selama sekian detik = link/GUI dianggap mati.
IDLE_TIMEOUT = 0.5


def resolve_manual_packet(axes, last_update, now, timeout=IDLE_TIMEOUT):
    """Tentukan paket MANUAL_CONTROL yang harus dikirim saat ini.

    Dipisah dari rov_agent.joystick_sender() supaya keputusan fail-safe bisa
    di-unit-test tanpa thread, socket, maupun jam dinding.

    axes        : dict axis GUI terakhir yang diterima (-1000..1000).
    last_update : time.time() saat axis terakhir masuk.
    now         : waktu sekarang.
    timeout     : batas diam sebelum dianggap stale.

    Return (packet, stale). Saat stale -> NEUTRAL (x/y/r = 0, z = 500), yaitu
    diam di tempat; di ALT_HOLD ini berarti menahan kedalaman, bukan tenggelam.
    """
    if not last_update or (now - last_update) > timeout:
        return dict(NEUTRAL), True

    return axes_to_manual_control(
        surge=axes.get("surge", 0),
        sway=axes.get("sway", 0),
        yaw=axes.get("yaw", 0),
        heave=axes.get("heave", 0),
    ), False