Menerapkan kalibrasi gerakan fisik untuk kontrol otonom dan memperbar…

README.md

@@ -311,11 +311,22 @@ di kolam yang disarankan.
 
 ### Tuning gerak autonomous dari GUI
 
-Buka **Setup → Autonomous Motion**, atur speed tiap fase (dive, ascend, surge,
-yaw, search, approach, engage, dan unhook), lalu tekan **Apply Autonomous Motion**
+Buka **Setup → Autonomous Motion**, atur target **Selam, Naik, Maju (m/s)** dan
+**Putar (°/s)**, lalu tekan **Apply Autonomous Motion**
 saat FSM belum berjalan. Mode pilot ArduSub **ALT_HOLD boleh tetap aktif**. Nilai berlaku
-pada start autonomous berikutnya; perubahan saat FSM sedang berjalan ditolak agar satu trial tetap konsisten. Batas input diterapkan
-lagi di Pi dan hanya mengubah command axis persen—mixer/PWM tetap milik ArduSub.
+pada start autonomous berikutnya; perubahan saat FSM sedang berjalan ditolak agar satu
+trial tetap konsisten. Pi menerjemahkan target fisik ke command berdasarkan
+`motion_calibration` di `autonomy/config/rov_tuned.yaml`, lalu membatasi command.
+Telemetry `surge_speed`, `vertical_speed`, dan `yaw_rate` dicatat untuk membandingkan
+target dengan gerak aktual. Mixer/PWM tetap milik ArduSub.
+Selama autonomous aktif, server juga menyalin snapshot log terbaru dari Pi ke
+`autonomy/logs/autonomous1.log`; file dapat diunduh dari panel Mission 5.
+
+Kalibrasi dilakukan dengan menjalankan beberapa command di kolam, mengukur jarak
+atau perubahan kedalaman terhadap waktu, lalu memperbarui empat nilai
+`motion_calibration`. Sebelum kalibrasi, angka target adalah estimasi command,
+bukan jaminan kecepatan nyata. Jika `LOCAL_POSITION_NED` tidak valid, `surge_speed`
+akan kosong dan tidak boleh dipakai sebagai bukti kecepatan maju.
 
 ### Deteksi QR robust + diagnosa (`decode_qr` & `--csv`)
 

autonomy/config/loader.py

@@ -45,6 +45,12 @@
     ('speed', 'surge'):   'SURGE_SPEED',
     ('speed', 'yaw'):     'YAW_SPEED',
 
+    # Kalibrasi fisik: kecepatan pada command axis 50, diukur di kolam.
+    ('motion_calibration', 'dive_mps_at_50'):  'DIVE_MPS_AT_50',
+    ('motion_calibration', 'ascend_mps_at_50'): 'ASCEND_MPS_AT_50',
+    ('motion_calibration', 'surge_mps_at_50'): 'SURGE_MPS_AT_50',
+    ('motion_calibration', 'yaw_dps_at_50'):   'YAW_DPS_AT_50',
+
     ('timeouts', 'dive'):      'TIMEOUT_DIVE',
     ('timeouts', 'scan'):      'TIMEOUT_SCAN',
     ('timeouts', 'grab'):      'TIMEOUT_GRAB',

autonomy/config/rov_tuned.yaml

@@ -26,6 +26,15 @@ invert:
   surge: false
   yaw: false
 
+# ── Kalibrasi gerak fisik (WAJIB diukur di kolam) ────────────────────────────
+# Kecepatan pada command axis 50. Nilai ini hanya titik awal sampai hasil uji
+# command→kecepatan dimasukkan; jangan menganggapnya sebagai spesifikasi motor.
+motion_calibration:
+  dive_mps_at_50: 0.20
+  ascend_mps_at_50: 0.20
+  surge_mps_at_50: 0.30
+  yaw_dps_at_50: 45.0
+
 # ── Gain PID servo docking — IBVS (piksel, tanpa kalibrasi kamera) ────────────
 pid_ibvs:
   kp_sway: 45.0

autonomy/fsm/mission5.py

@@ -67,10 +67,18 @@
 HOOK_HEIGHT_FROM_FLOOR = None  # m — tinggi ujung hook dari DASAR (KKI 2026 = 0.45)
 BOTTOM_CLEARANCE       = None  # m — jarak aman titik-tengah ROV di atas dasar (BERPINDAH antar venue)
 
-DIVE_SPEED            = 30     # % thruster vertikal saat menyelam
-ASCEND_SPEED          = 30     # % thruster vertikal saat naik
-SURGE_SPEED           = 35     # % surge saat navigasi horizontal
-YAW_SPEED             = 25     # % yaw saat rotasi
+DIVE_SPEED            = 30     # command axis (0..100) saat menyelam
+ASCEND_SPEED          = 30     # command axis (0..100) saat naik
+SURGE_SPEED           = 35     # command axis (0..100) saat navigasi horizontal
+YAW_SPEED             = 25     # command axis (0..100) saat rotasi
+
+# Kalibrasi fisik: estimasi kecepatan pada command axis 50. Nilai ini WAJIB
+# diganti dengan hasil uji kolam; ia bukan spesifikasi thruster dan tidak
+# membuktikan kecepatan nyata sebelum telemetry kecepatan tersedia.
+DIVE_MPS_AT_50        = 0.20   # m/s turun pada command 50
+ASCEND_MPS_AT_50      = 0.20   # m/s naik pada command 50
+SURGE_MPS_AT_50       = 0.30   # m/s maju pada command 50
+YAW_DPS_AT_50         = 45.0   # deg/s pada command 50
 
 # SCAN_QR dulu cuma yaw di tempat menunggu decode penuh — di air keruh QR baru terbaca
 # dari jarak jauh lebih dekat drpd air jernih (24 Agu: foto lapangan gagal decode walau QR
@@ -668,6 +676,9 @@ def _log_sample(self, telem):
         self.runlog.event('sample',
                           state=t['state'], active_cam=t['active_cam'],
                           depth=telem.get('depth'), heading=telem.get('heading'),
+                          surge_speed=telem.get('surge_speed'),
+                          vertical_speed=telem.get('vertical_speed'),
+                          yaw_rate=telem.get('yaw_rate'),
                           distance_z=t['distance_z'],
                           offset_x=t['offset_x'], offset_y=t['offset_y'],
                           qr_data=t['qr_data'], qr_wall=t['qr_wall'],

autonomy/rov_link.py

@@ -26,7 +26,8 @@
 
 Kontrak JSON (sesuai server.js + README-WORK §3):
   Command masuk  : {"name": "...", "value": ..., "t": ...}
-  Telemetri keluar: {heading, roll, pitch, depth, temp, voltage, armed, light, mode, ts}
+  Telemetri keluar: {heading, roll, pitch, depth, surge_speed, vertical_speed,
+                     yaw_rate, temp, voltage, armed, light, mode, ts}
 """
 
 import argparse
@@ -156,6 +157,7 @@ def __init__(self, args):
         # telemetri terbaru hasil parsing MAVLink
         self.telem = {
             "heading": None, "roll": None, "pitch": None, "depth": None,
+            "surge_speed": None, "vertical_speed": None, "yaw_rate": None,
             "temp": None, "voltage": None, "armed": False,
             "light": False, "mode": "manual", "poshold": False,
         }
@@ -195,6 +197,13 @@ def _request_streams(self):
         self.master.mav.request_data_stream_send(
             self.master.target_system, self.master.target_component,
             mavutil.mavlink.MAV_DATA_STREAM_ALL, 10, 1)  # 10 Hz
+        # Be explicit: some ArduSub/SITL configurations ignore the broad
+        # stream request for LOCAL_POSITION_NED.
+        self.master.mav.command_long_send(
+            self.master.target_system, self.master.target_component,
+            mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
+            mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED,
+            100000, 0, 0, 0, 0, 0, 0)
 
     def arm(self, on):
         self.master.mav.command_long_send(
@@ -406,6 +415,21 @@ def loop_mavlink_rx(self):
                 self.telem["roll"] = round(math.degrees(msg.roll), 1)
                 self.telem["pitch"] = round(math.degrees(msg.pitch), 1)
                 self.telem["heading"] = round((math.degrees(msg.yaw) + 360) % 360, 1)
+                self.telem["yaw_rate"] = round(math.degrees(msg.yawspeed), 3)
+            elif t == "LOCAL_POSITION_NED":
+                # MAVLink LOCAL_POSITION_NED velocity: cm/s, NED frame.
+                try:
+                    vn = float(msg.vx) / 100.0
+                    ve = float(msg.vy) / 100.0
+                    vd = float(msg.vz) / 100.0
+                    if not all(math.isfinite(v) for v in (vn, ve, vd)):
+                        raise ValueError("velocity bukan finite")
+                    hdg = math.radians(float(self.telem["heading"] or 0.0))
+                    self.telem["surge_speed"] = round(vn * math.cos(hdg) + ve * math.sin(hdg), 4)
+                    self.telem["vertical_speed"] = round(vd, 4)
+                except (TypeError, ValueError):
+                    self.telem["surge_speed"] = None
+                    self.telem["vertical_speed"] = None
             elif t == "SCALED_PRESSURE2":
                 self.last_press_abs = msg.press_abs
                 depth = (msg.press_abs - self.surface_hpa) * 100.0 / (WATER_RHO * G)

public/index.html

@@ -334,6 +334,7 @@
               <div class="readout"><span class="readout__k">SKOR</span><span class="readout__v" id="runLastScore">—</span><span class="readout__u">/100</span></div>
               <div class="readout"><span class="readout__k">DURASI</span><span class="readout__v" id="runLastDur">—</span><span class="readout__u">s</span></div>
               <div class="readout"><span class="readout__k">QR RATE</span><span class="readout__v" id="runLastQr">—</span><span class="readout__u">%</span></div>
+              <a class="card__link" href="/api/autonomous1.log" download="autonomous1.log">Download autonomous1.log</a>
             </section>
 
             <!-- LIVE CAMERA -->
@@ -431,4 +432,4 @@
   <script src="vendor/jsqr.min.js"></script>
   <script type="module" src="js/app.js"></script>
 </body>
-</html>
+</html>

public/js/app.js

@@ -6,7 +6,7 @@ import { telemetryPage } from "./pages/telemetry.js";
 import { missionPage } from "./pages/mission.js";
 import { cameraPage } from "./pages/camera.js";
 import { replayPage } from "./pages/replay.js";
-import { setupPage, loadSetup } from "./pages/setup.js";
+import { setupPage, loadSetup, autonomyMotionConfig } from "./pages/setup.js";
 import { vehiclePage } from "./pages/vehicle.js";
 import { analyzePage } from "./pages/analyze.js";
 import { joystickPage,handleJoystickConfigMessage} from "./pages/joystick.js";
@@ -836,7 +836,7 @@ function connect() {
        kalau prosesnya sempat restart. */
     if (Number.isFinite(CONFIG.POOL_DEPTH)) sendCmd("pool_depth", CONFIG.POOL_DEPTH, true);
     if (CONFIG.AUTONOMY_MOTION_CONFIGURED && CONFIG.AUTONOMY_MOTION) {
-      sendCmd("mission5_motion", CONFIG.AUTONOMY_MOTION, true);
+      sendCmd("mission5_motion", autonomyMotionConfig(CONFIG.AUTONOMY_MOTION), true);
     }
   };
   ws.onclose = () => {

public/js/config.js

@@ -64,12 +64,10 @@ export const CONFIG = {
     KEY_AXIS_STEP: 400,
   },
 
-  // Batas awal gerak FSM autonomous dalam persen command axis. Ini bukan
-  // mixer/PWM; ArduSub tetap mengurus stabilisasi dan mixing.
+  // Target gerak FSM autonomous dalam satuan fisik. Pi mengubahnya menjadi
+  // command axis memakai kalibrasi kolam; ini bukan mixer/PWM.
   AUTONOMY_MOTION: {
-    dive: 30, ascend: 30, surge: 35, yaw: 25,
-    scan_creep: 18, search: 20, approach: 20, engage: 15,
-    unhook_vert: 30, unhook_surge: -20,
+    dive: 0.12, ascend: 0.12, surge: 0.21, yaw: 22.5,
   },
   AUTONOMY_MOTION_CONFIGURED: false,
 

public/js/pages/setup.js

@@ -67,6 +67,7 @@ function saveSetup() {
       TEAM_NAME: CONFIG.TEAM_NAME, UNIVERSITY: CONFIG.UNIVERSITY,
       CAMERAS: CONFIG.CAMERAS, THRUSTER: CONFIG.THRUSTER,
       POOL_DEPTH: CONFIG.POOL_DEPTH, DANGER_DEPTH: CONFIG.DANGER_DEPTH,
+      AUTONOMY_MOTION_UNITS: "physical-v1",
       AUTONOMY_MOTION: CONFIG.AUTONOMY_MOTION_CONFIGURED ? CONFIG.AUTONOMY_MOTION : null,
     }));
     /* PID SENGAJA TIDAK ikut disimpan: sumber kebenarannya sekarang flight
@@ -91,7 +92,13 @@ export function loadSetup() {
     if (s.AUTONOMY_MOTION && typeof s.AUTONOMY_MOTION === "object") {
       CONFIG.AUTONOMY_MOTION_CONFIGURED = true;
       for (const field of MOTION_FIELDS) {
-        const value = Number(s.AUTONOMY_MOTION[field.key]);
+        const stored = Number(s.AUTONOMY_MOTION[field.key]);
+        // Migrasi nilai versi level/command lama ke target fisik nominal.
+        const value = s.AUTONOMY_MOTION_UNITS === "physical-v1"
+          ? stored
+          : stored >= 0 && stored <= 5
+            ? stored / 5 * field.defaultMax
+            : stored / field.maxCommand * field.defaultMax;
         if (Number.isFinite(value) && value >= field.min && value <= field.max) {
           CONFIG.AUTONOMY_MOTION[field.key] = value;
         }
@@ -108,22 +115,22 @@ const numField = (id, label, val, step = "1", unit = "") => `
     <input id="${id}" type="number" step="${step}" value="${val}" /></label>`;
 
 const MOTION_FIELDS = [
-  { key: "dive", label: "Dive", min: 0, max: 50 },
-  { key: "ascend", label: "Ascend", min: 0, max: 50 },
-  { key: "surge", label: "Surge", min: 0, max: 50 },
-  { key: "yaw", label: "Yaw", min: 0, max: 50 },
-  { key: "scan_creep", label: "Scan creep", min: 0, max: 35 },
-  { key: "search", label: "Search", min: 0, max: 35 },
-  { key: "approach", label: "Approach", min: 0, max: 35 },
-  { key: "engage", label: "Engage", min: 0, max: 30 },
-  { key: "unhook_vert", label: "Unhook vert", min: 0, max: 40 },
-  { key: "unhook_surge", label: "Unhook surge", min: -40, max: 0 },
+  { key: "dive", label: "Selam", unit: "m/s", min: 0, max: 0.20, step: "0.01", defaultMax: 0.20, maxCommand: 50 },
+  { key: "ascend", label: "Naik", unit: "m/s", min: 0, max: 0.20, step: "0.01", defaultMax: 0.20, maxCommand: 50 },
+  { key: "surge", label: "Maju", unit: "m/s", min: 0, max: 0.30, step: "0.01", defaultMax: 0.30, maxCommand: 50 },
+  { key: "yaw", label: "Putar", unit: "°/s", min: 0, max: 45, step: "1", defaultMax: 45, maxCommand: 50 },
 ];
 
+export function autonomyMotionConfig(values) {
+  return Object.fromEntries(MOTION_FIELDS.map((field) => [
+    field.key, Math.max(field.min, Math.min(field.max, Number(values[field.key]) || 0)),
+  ]));
+}
+
 const motionField = (field, values) => `
-  <label class="field field--sm"><span>${field.label} <small>%</small></span>
+  <label class="field field--sm"><span>${field.label} <small>${field.unit}</small></span>
     <input id="suMotion${field.key}" type="number" min="${field.min}" max="${field.max}"
-           step="1" value="${values[field.key]}" inputmode="decimal" /></label>`;
+           step="${field.step}" value="${values[field.key] ?? 0}" inputmode="decimal" /></label>`;
 
 // resolusi umum yang didukung mjpg-streamer via input_uvc.so -r; daftar tidak
 // divalidasi terhadap kemampuan kamera fisik (lihat autonomy/tools/pi_restart_camera.sh)
@@ -289,11 +296,15 @@ export const setupPage = {
           <div class="card">
             <span class="panel__eyebrow">AUTONOMOUS MOTION</span>
             <h3 class="card__title">Mission 5 Movement</h3>
+            <p class="card__desc">Atur target gerak nyata: Selam, Naik, dan Maju dalam m/s;
+              Putar dalam °/s. Nilai ini diterjemahkan ke command ArduSub memakai kalibrasi
+              kolam dan dibatasi ulang di Pi.</p>
             <div class="card__row card__row--wrap">
               ${MOTION_FIELDS.map((field) => motionField(field, A)).join("")}
             </div>
             <button class="btn-wide" id="suApplyMotion">Apply Autonomous Motion</button>
             <span class="card__info" id="suMotionInfo">Batas aman diterapkan di Pi</span>
+            <span class="card__info" id="suMotionActual">Aktual: menunggu telemetry</span>
           </div>
 
           <!-- MISSION SCORING -->
@@ -717,18 +728,20 @@ root.querySelector("#suGainDown")?.addEventListener("click", () => {
       log(`Pool ${CONFIG.POOL_DEPTH.toFixed(2)} m, danger ${CONFIG.DANGER_DEPTH.toFixed(2)} m`, "ok");
     };
 
-    /* AUTONOMOUS MOTION — hanya tuning bounded axis, bukan PID FC/mixer. */
+    /* AUTONOMOUS MOTION — target fisik dikirim ke Pi; Pi mengonversinya ke
+       bounded axis memakai kalibrasi, bukan PID FC/mixer. */
     this.els.motionInputs = Object.fromEntries(
       MOTION_FIELDS.map((field) => [field.key, root.querySelector(`#suMotion${field.key}`)])
     );
     this.els.motionInfo = root.querySelector("#suMotionInfo");
+    this.els.motionActual = root.querySelector("#suMotionActual");
     root.querySelector("#suApplyMotion").onclick = () => {
       const next = {};
       const invalid = [];
       for (const field of MOTION_FIELDS) {
         const value = Number(this.els.motionInputs[field.key].value);
         if (!Number.isFinite(value) || value < field.min || value > field.max) {
-          invalid.push(`${field.label} ${field.min}..${field.max}`);
+          invalid.push(`${field.label} ${field.min}..${field.max} ${field.unit}`);
         } else {
           next[field.key] = value;
         }
@@ -740,7 +753,7 @@ root.querySelector("#suGainDown")?.addEventListener("click", () => {
       CONFIG.AUTONOMY_MOTION = next;
       CONFIG.AUTONOMY_MOTION_CONFIGURED = true;
       saveSetup();
-      sendCmd("mission5_motion", next);
+      sendCmd("mission5_motion", autonomyMotionConfig(next));
       if (this.els.motionInfo) this.els.motionInfo.textContent = "Terkirim — berlaku pada start berikutnya";
       log("Tuning gerak autonomous dikirim", "ok");
     };
@@ -809,6 +822,13 @@ root.querySelector("#suGainDown")?.addEventListener("click", () => {
       }
     }
 
+    if (this.els.motionActual) {
+      const fmt = (value, unit) => Number.isFinite(Number(value))
+        ? `${Number(value).toFixed(unit === "°/s" ? 1 : 3)} ${unit}` : "—";
+      this.els.motionActual.textContent = `Aktual: maju ${fmt(d.surge_speed, "m/s")} · `
+        + `vertikal ${fmt(d.vertical_speed, "m/s")} · putar ${fmt(d.yaw_rate, "°/s")}`;
+    }
+
     const mc = d.mission_counter;
     if (!mc || !this.els.suM2Fails) return;
     this.els.suM2Fails.textContent = mc.m2_fails + 1;

rov_agent.py

@@ -131,6 +131,12 @@ def qgc_command_receiver():
 # =========================
 state = {
     "heading": 0.0,
+    # Kecepatan dari LOCAL_POSITION_NED (m/s, frame NED) dan ATTITUDE (deg/s).
+    # None berarti FC belum menyediakan estimasi yang bisa dipercaya.
+    "vel_n": None,
+    "vel_e": None,
+    "vel_d": None,
+    "yaw_rate": None,
     "depth": 0.0,       # sementara 0 dulu, nanti kita isi dari sensor depth
     "roll": 0.0,
     "pitch": 0.0,
@@ -543,6 +549,17 @@ def send_telemetry():
 
     state["pool_depth"] = pool_depth
 
+    # Kecepatan body yang bisa dibandingkan langsung dengan target Setup.
+    # Surge dihitung dari velocity NED + heading; bila EKF tidak punya estimasi
+    # horizontal, nilainya None, bukan 0 palsu. NED: down positif.
+    vn, ve = state.get("vel_n"), state.get("vel_e")
+    if all(isinstance(v, (int, float)) and math.isfinite(v) for v in (vn, ve)):
+        hdg = math.radians(float(state.get("heading", 0.0)))
+        state["surge_speed"] = round(vn * math.cos(hdg) + ve * math.sin(hdg), 4)
+    else:
+        state["surge_speed"] = None
+    state["vertical_speed"] = state.get("vel_d")
+
     # Gate otoritas untuk mission5 FSM (toggle autonomous/manual di GUI).
     # HARUS kunci sendiri: dulu ini menulis ke state["mode"] dan menimpa pilot
     # mode ArduSub dari HEARTBEAT 10x/detik. Akibatnya requested_mode tak pernah
@@ -1835,6 +1852,21 @@ def connect_pixhawk():
     except Exception as e:
         print("[MAV] request_data_stream_send warning:", e)
 
+    # Minta velocity EKF secara eksplisit. MAV_DATA_STREAM_ALL tidak selalu
+    # dihormati oleh semua firmware/parameter ArduSub.
+    try:
+        link.mav.command_long_send(
+            link.target_system,
+            link.target_component,
+            mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
+            0,
+            mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED,
+            100000,      # 10 Hz (100000 µs)
+            0, 0, 0, 0, 0, 0
+        )
+    except Exception as e:
+        print("[MAV] LOCAL_POSITION_NED request warning:", e)
+
     # Request AHRS2 (Depth)
     try:
         link.mav.command_long_send(
@@ -2018,6 +2050,7 @@ def main():
             state["roll"] = roll_f
             state["pitch"] = pitch_f
             state["heading"] = yaw_f
+            state["yaw_rate"] = math.degrees(msg.yawspeed)
             prev_attitude_ts = now_ts
 
         # --------------------------------
@@ -2027,6 +2060,16 @@ def main():
         elif mtype == "LOCAL_POSITION_NED":
             state["pos_n"] = float(msg.x)
             state["pos_e"] = float(msg.y)
+            # MAVLink mengirim posisi dalam meter dan velocity dalam cm/s.
+            # Beberapa FC tidak mengisi velocity; pertahankan None agar GUI
+            # tidak menampilkan angka palsu sebagai kecepatan ROV.
+            for field, source in (("vel_n", "vx"), ("vel_e", "vy"), ("vel_d", "vz")):
+                value = getattr(msg, source, None)
+                try:
+                    value = float(value) / 100.0
+                except (TypeError, ValueError):
+                    value = None
+                state[field] = value if value is not None and math.isfinite(value) else None
 
         # --------------------------------
         # PARAM_VALUE: tabel param (halaman Vehicle) + verifikasi param_set.