diff --git a/README.md b/README.md index 99b428cd..049bb01f 100644 --- a/README.md +++ b/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`) diff --git a/autonomy/config/loader.py b/autonomy/config/loader.py index 4fd34a0a..08c6aa12 100644 --- a/autonomy/config/loader.py +++ b/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', diff --git a/autonomy/config/rov_tuned.yaml b/autonomy/config/rov_tuned.yaml index 26853062..cafeeef0 100644 --- a/autonomy/config/rov_tuned.yaml +++ b/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 diff --git a/autonomy/fsm/mission5.py b/autonomy/fsm/mission5.py index 8469b499..05d2a17b 100644 --- a/autonomy/fsm/mission5.py +++ b/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'], diff --git a/autonomy/rov_link.py b/autonomy/rov_link.py index 7159c2eb..adbb9341 100644 --- a/autonomy/rov_link.py +++ b/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) diff --git a/public/index.html b/public/index.html index 675029fc..6538586a 100644 --- a/public/index.html +++ b/public/index.html @@ -334,6 +334,7 @@
SKOR—/100
DURASI—s
QR RATE—%
+ Download autonomous1.log @@ -431,4 +432,4 @@ - \ No newline at end of file + diff --git a/public/js/app.js b/public/js/app.js index b8b07bd5..49c4fbb3 100644 --- a/public/js/app.js +++ b/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 = () => { diff --git a/public/js/config.js b/public/js/config.js index 4ede80c9..a2ba7ee6 100644 --- a/public/js/config.js +++ b/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, diff --git a/public/js/pages/setup.js b/public/js/pages/setup.js index c91760cd..4ff8306f 100644 --- a/public/js/pages/setup.js +++ b/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 = "") => ` `; 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) => ` - `; // 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 = {
AUTONOMOUS MOTION

Mission 5 Movement

+

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.

${MOTION_FIELDS.map((field) => motionField(field, A)).join("")}
Batas aman diterapkan di Pi + Aktual: menunggu telemetry
@@ -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; diff --git a/rov_agent.py b/rov_agent.py index ce7746d3..ed1f2364 100644 --- a/rov_agent.py +++ b/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. diff --git a/rov_mission5_bridge.py b/rov_mission5_bridge.py index bf1c922f..ad464c63 100644 --- a/rov_mission5_bridge.py +++ b/rov_mission5_bridge.py @@ -45,31 +45,26 @@ import math -# Runtime tuning yang aman untuk gerak autonomous. Ini tetap command axis (%) -# ke ArduSub, bukan PWM dan bukan mixer baru. +# Runtime tuning gerak autonomous dari GUI memakai satuan fisik. Nilai ini +# dikonversi ke command axis oleh Pi memakai kalibrasi command→kecepatan; +# ArduSub tetap menjadi pemilik mixer/PWM. MOTION_LIMITS = { - "dive": (0.0, 50.0), - "ascend": (0.0, 50.0), - "surge": (0.0, 50.0), - "yaw": (0.0, 50.0), - "scan_creep": (0.0, 35.0), - "search": (0.0, 35.0), - "approach": (0.0, 35.0), - "engage": (0.0, 30.0), - "unhook_vert": (0.0, 40.0), - "unhook_surge": (-40.0, 0.0), + "dive": (0.0, 0.20), # m/s, arah turun + "ascend": (0.0, 0.20), # m/s, arah naik + "surge": (0.0, 0.30), # m/s, maju + "yaw": (0.0, 45.0), # deg/s } MOTION_CONSTANTS = { "dive": "DIVE_SPEED", "ascend": "ASCEND_SPEED", "surge": "SURGE_SPEED", "yaw": "YAW_SPEED", - "scan_creep": "SCAN_CREEP_MAX_SPEED", - "search": "SEARCH_SPEED", - "approach": "DOCK_APPROACH_SPEED", - "engage": "M5_ENGAGE_SURGE", - "unhook_vert": "M5_UNHOOK_VERT", - "unhook_surge": "M5_UNHOOK_SURGE", +} +MOTION_CALIBRATION = { + "dive": "DIVE_MPS_AT_50", + "ascend": "ASCEND_MPS_AT_50", + "surge": "SURGE_MPS_AT_50", + "yaw": "YAW_DPS_AT_50", } @@ -87,7 +82,8 @@ def validate_motion_config(values): return {}, f"{key} bukan angka" lo, hi = MOTION_LIMITS[key] if not math.isfinite(number) or not lo <= number <= hi: - return {}, f"{key} di luar batas {lo:g}..{hi:g}%" + unit = "°/s" if key == "yaw" else "m/s" + return {}, f"{key} di luar batas {lo:g}..{hi:g} {unit}" out[key] = number return out, None @@ -257,8 +253,10 @@ def motion_config(self): """Konfigurasi gerak efektif, termasuk default/config file terakhir.""" try: import fsm.mission5 as m5 - result = {key: getattr(m5, attr) - for key, attr in MOTION_CONSTANTS.items()} + result = { + key: getattr(m5, attr) / 50.0 * getattr(m5, MOTION_CALIBRATION[key]) + for key, attr in MOTION_CONSTANTS.items() + } except Exception: result = {} result.update(self._cfg.get("runtime_motion", {})) @@ -270,8 +268,16 @@ def _apply_runtime_motion(self): return import fsm.mission5 as m5 for key, value in runtime.items(): - setattr(m5, MOTION_CONSTANTS[key], value) - self._log(f"[M5] tuning gerak runtime diterapkan: {runtime}") + calibration = float(getattr(m5, MOTION_CALIBRATION[key])) + if not math.isfinite(calibration) or calibration <= 0: + raise ValueError(f"kalibrasi {key} tidak valid: {calibration}") + # Nilai GUI adalah target gerak; FSM tetap menerima command lama + # -100..100 agar visual servo dan safety envelope tidak berubah. + command = round(float(value) / calibration * 50.0) + command = max(0, min(50, command)) + setattr(m5, MOTION_CONSTANTS[key], command) + self._log(f"[M5] {key}={value:g} -> command={command} " + f"(kalibrasi {calibration:g}/50)") def start(self): """Nyalakan FSM. Aman dipanggil berulang — start kedua diabaikan. @@ -296,7 +302,11 @@ def start(self): return False self._apply_configs() - self._apply_runtime_motion() + try: + self._apply_runtime_motion() + except (TypeError, ValueError) as e: + self._log(f"[M5] TIDAK BISA START — kalibrasi gerak tidak valid: {e}") + return False cfg = self._cfg @@ -320,7 +330,16 @@ def start(self): _time.strftime("run_%Y%m%d_%H%M%S.jsonl")) runlog = RunLogger(log_path) files = [p.strip() for p in (cfg.get("config_files") or "").split(",") if p.strip()] - runlog.event("config", files=files, start_state=cfg.get("start_state", "M5_REDIVE")) + import fsm.mission5 as _m5 + runlog.event( + "config", + files=files, + start_state=cfg.get("start_state", "M5_REDIVE"), + motion_target=self.motion_config(), + motion_calibration={ + key: getattr(_m5, attr) for key, attr in MOTION_CALIBRATION.items() + }, + ) except Exception as e: self._log(f"[M5] run_log tidak tersedia: {e} — trial tetap jalan tanpa log") self._runlog = runlog diff --git a/server/server.js b/server/server.js index e0bd2ece..9667696d 100644 --- a/server/server.js +++ b/server/server.js @@ -33,6 +33,52 @@ const SIM = process.argv.includes("--sim"); const PUBLIC = path.join(__dirname, "..", "public"); const SHARED_ROOT = path.join(__dirname, "..", "shared"); const AUTONOMY = path.join(__dirname, "..", "autonomy"); +const AUTONOMOUS_LOG = path.join(AUTONOMY, "logs", "autonomous1.log"); +fs.mkdirSync(path.dirname(AUTONOMOUS_LOG), { recursive: true }); + +let lastControlMode = null; +let autonomousLogTimer = null; +let autonomousLogSyncBusy = false; + +function syncLatestAutonomousLog() { + if (autonomousLogSyncBusy) return; + autonomousLogSyncBusy = true; + // Salin snapshot terbaru selama autonomous berjalan. RunLogger di Pi flush + // setiap event, jadi file lokal bisa dipakai untuk monitoring sebelum trial + // selesai; nama tetap autonomous1.log agar alat evaluasi punya satu target. + execFile("rsync", [ + "-az", + "-e", "ssh -o BatchMode=yes -o ConnectTimeout=3 -o StrictHostKeyChecking=accept-new", + `${RPI_SSH_USER}@${RPI_ADDR}:${RPI_LOG_DIR}/*.jsonl`, + path.join(AUTONOMY, "logs"), + ], { timeout: 5000 }, () => { + fs.readdir(path.dirname(AUTONOMOUS_LOG), (err, names) => { + const runs = err ? [] : names.filter((name) => /^run_.*\.jsonl$/.test(name)).sort(); + const latest = runs[runs.length - 1]; + if (!latest) { + autonomousLogSyncBusy = false; + return; + } + fs.copyFile(path.join(path.dirname(AUTONOMOUS_LOG), latest), AUTONOMOUS_LOG, () => { + autonomousLogSyncBusy = false; + }); + }); + }); +} + +function trackAutonomousRun(data) { + const mode = data && data.control_mode; + if (mode === "autonomous" && lastControlMode !== "autonomous") { + syncLatestAutonomousLog(); + clearInterval(autonomousLogTimer); + autonomousLogTimer = setInterval(syncLatestAutonomousLog, 2000); + } else if (mode !== "autonomous" && lastControlMode === "autonomous") { + clearInterval(autonomousLogTimer); + autonomousLogTimer = null; + syncLatestAutonomousLog(); + } + if (typeof mode === "string") lastControlMode = mode; +} const MOTION_AXES = new Set([ "surge", @@ -354,6 +400,18 @@ const httpServer = http.createServer((req, res) => { () => runAnalyze()); } + if (urlPath === "/api/autonomous1.log") { + return fs.readFile(AUTONOMOUS_LOG, (err, data) => { + if (err) { res.writeHead(404); return res.end("Belum ada autonomous1.log"); } + res.writeHead(200, { + "Content-Type": "text/plain; charset=utf-8", + "Content-Disposition": "attachment; filename=autonomous1.log", + "Cache-Control": "no-store", + }); + res.end(data); + }); + } + // Satu frame JPEG dari sesi: /replay/frame?session=&cam=&i= if (urlPath === "/replay/frame") { const q = new URL(req.url, `http://localhost:${WS_PORT}`).searchParams; @@ -420,7 +478,10 @@ const clients = new Set(); function broadcast(obj) { // Tap telemetry untuk rekaman trajectory (bila sesi rekam aktif). Ini hanya // "menguping" — tidak mengubah/menghambat aliran telemetry ke dashboard. - if (obj && obj.type === "telemetry") recording.onTelemetry(obj.data); + if (obj && obj.type === "telemetry") { + recording.onTelemetry(obj.data); + trackAutonomousRun(obj.data); + } const s = JSON.stringify(obj); for (const c of clients) { if (c.readyState === 1) c.send(s); @@ -1113,4 +1174,4 @@ async function start() { }); } -start(); \ No newline at end of file +start(); diff --git a/test_rov_mission5_bridge.py b/test_rov_mission5_bridge.py index 51c8ce50..c8371b4b 100644 --- a/test_rov_mission5_bridge.py +++ b/test_rov_mission5_bridge.py @@ -138,16 +138,16 @@ class TestAutonomousMotionConfig(unittest.TestCase): def test_tuning_gerak_valid_disimpan(self): r = Mission5Runner(_adapter({}), Mission5TelemetryAdapter(lambda: {}), log=lambda *_: None) - ok, cfg = r.update_motion_config({"dive": 40, "unhook_surge": -25}) + ok, cfg = r.update_motion_config({"dive": 0.16, "yaw": 20}) self.assertTrue(ok) - self.assertEqual(r._cfg["runtime_motion"]["dive"], 40.0) - self.assertEqual(r._cfg["runtime_motion"]["unhook_surge"], -25.0) - self.assertEqual(cfg["dive"], 40.0) + self.assertEqual(r._cfg["runtime_motion"]["dive"], 0.16) + self.assertEqual(r._cfg["runtime_motion"]["yaw"], 20.0) + self.assertEqual(cfg["dive"], 0.16) def test_tuning_gerak_di_luar_batas_ditolak(self): r = Mission5Runner(_adapter({}), Mission5TelemetryAdapter(lambda: {}), log=lambda *_: None) - ok, reason = r.update_motion_config({"dive": 51}) + ok, reason = r.update_motion_config({"dive": 0.21}) self.assertFalse(ok) self.assertIn("di luar batas", reason) self.assertNotIn("runtime_motion", r._cfg)