diff --git a/Dev/index.html b/Dev/index.html index 618af763..8796cc07 100644 --- a/Dev/index.html +++ b/Dev/index.html @@ -108,6 +108,18 @@

Thrust Expo

+ + + + + +

Simple GCS

+ An example simple web based GCS. Setup for small boats + (eg. autonomous buoy). Supports wss to connect via ArduPilot + support server. + + + diff --git a/SimpleGCS/app.js b/SimpleGCS/app.js new file mode 100644 index 00000000..f2e76734 --- /dev/null +++ b/SimpleGCS/app.js @@ -0,0 +1,928 @@ +/* + app.js — Simple, mobile-friendly GCS map for autonomous buoys + */ + +(() => { + // --- State --- + let MAVLink = new MAVLink20Processor(); // provided by ../modules/MAVLink/mavlink.js + // IDs we send as (GCS identity) + let gcsSystemId = 255, gcsComponentId = 190; + // Target vehicle (auto-discovered from incoming msgs) + let vehSysId = 1, vehCompId = 1; + + // Vehicle type cache (for icon) + const VehicleType = { mavType: null, cls: "plane", lastSeen: 0 }; + + // Map + marker + const map = L.map(document.getElementById("map"), { zoomControl: true }).setView([0,0], 2); + L.tileLayer("https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png", { maxZoom: 19, attribution: "© OpenStreetMap" }).addTo(map); + + // prevent iPhone popup menus + map.getContainer().addEventListener("contextmenu", (e) => e.preventDefault()); + + let vehicleMarker = null; + let appliedVehClass = null; + let lastHeadingDeg = null; + + // marker for vehicle target + let targetMarker = null; + + // Toolbar + const armBtn = document.getElementById("armBtn"); + const disarmBtn = document.getElementById("disarmBtn"); + const rtlBtn = document.getElementById("rtlBtn"); + const loiterBtn = document.getElementById("loiterBtn"); + const recenterBtn = document.getElementById("recenterBtn"); + const connectBtn = document.getElementById("connectBtn"); + + // Connection + let ws = null; + let hbInterval = null; + let setupSigning = false; + + const roverModes = { + MANUAL : 0, + ACRO : 1, + STEERING : 3, + HOLD : 4, + LOITER : 5, + FOLLOW : 6, + SIMPLE : 7, + DOCK : 8, + CIRCLE : 9, + AUTO : 10, + RTL : 11, + SMART_RTL : 12, + GUIDED : 15, + INITIALISING : 16, + }; + + // --- Helpers --- + function toast(msg, ms=1500) { + const el = document.createElement("div"); + el.className = "toast"; + el.textContent = msg; + document.body.appendChild(el); + setTimeout(() => el.remove(), ms); + } + + // --- COMMAND_INT helper --- + function sendCommandInt(cmd, params = []) { + if (!ws) { toast("Not connected"); return; } + const payload = new mavlink20.messages.command_int( + vehSysId, // target_system + vehCompId, // target_component + mavlink20.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + cmd, // command + 0, 0, + params[0] || 0, params[1] || 0, params[2] || 0, params[3] || 0, + params[4] || 0, params[5] || 0, params[6] || 0 + ); + const pkt = payload.pack(MAVLink); + ws.send(Uint8Array.from(pkt)); + } + + function sendSetMode(mode) { + if (!ws) { toast("Not connected"); return; } + sendCommandInt(mavlink20.MAV_CMD_DO_SET_MODE, [ mavlink20.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode ]); + } + + // Telemetry state + let telemetry = { + batteryPct: null, + currentA: null, + speed: 0, + lastUpdate: 0, + armed: false, + modeName: "—" + }; + + // make rover mode names map + const roverModeNames = Object.fromEntries(Object.entries(roverModes).map(([k,v]) => [v, k])); + + let lastTargetSeenMs = 0; + + // Create telemetry display elements + function initTelemetryDisplay() { + // Find the toolbar + const toolbar = document.getElementById("toolbar"); + + // Create telemetry container + const telemetryDiv = document.createElement("div"); + telemetryDiv.id = "telemetry"; + telemetryDiv.style.cssText = ` + background: rgba(255,255,255,0.1); + border-radius: 8px; + padding: 8px; + margin: 10px 0; + font-size: 11px; + text-align: center; + color: #fff; + width: calc(var(--barW) - 12px); + `; + + // Status (armed + mode) + const statusDiv = document.createElement("div"); + statusDiv.id = "status-display"; + statusDiv.style.cssText = "margin-bottom: 8px;"; + statusDiv.innerHTML = ` +
+ + DISARM +
+
MODE: —
+`; + telemetryDiv.prepend(statusDiv); + + // Battery display + const batteryDiv = document.createElement("div"); + batteryDiv.id = "battery-display"; + batteryDiv.style.cssText = "margin-bottom: 6px;"; + batteryDiv.innerHTML = ` +
BATTERY
+
---%
+
--- A
+`; + + // Speed display + const speedDiv = document.createElement("div"); + speedDiv.id = "speed-display"; + speedDiv.innerHTML = ` +
SPEED
+
--- knots
+ `; + + telemetryDiv.appendChild(batteryDiv); + telemetryDiv.appendChild(speedDiv); + + // Insert before the flex spacer + const spacer = toolbar.querySelector('div[style*="flex:1"]'); + toolbar.insertBefore(telemetryDiv, spacer); + } + + function updateTelemetryDisplay() { + const batteryEl = document.getElementById("battery-value"); + const currentEl = document.getElementById("current-value"); + const speedEl = document.getElementById("speed-value"); + const armedEl = document.getElementById("armed-pill"); + const modeEl = document.getElementById("mode-value"); + + // battery + if (batteryEl) { + if (telemetry.batteryPct !== null && telemetry.batteryPct >= 0) { + let color = "#4caf50"; + if (telemetry.batteryPct < 20) color = "#f44336"; + else if (telemetry.batteryPct < 40) color = "#ff9800"; + batteryEl.textContent = `${telemetry.batteryPct}%`; + batteryEl.style.color = color; + } else { + batteryEl.textContent = "---"; + batteryEl.style.color = "#fff"; + } + } + + if (currentEl) { + if (telemetry.currentA !== null && telemetry.currentA >= 0) { + currentEl.textContent = `${telemetry.currentA.toFixed(1)} A`; + } else { + currentEl.textContent = "--- A"; + } + } + + // speed + if (speedEl) { + if (telemetry.speed >= 0) { + var speed_knots = 1.94384449 * telemetry.speed; + speedEl.textContent = `${speed_knots.toFixed(1)} knots`; + } else { + speedEl.textContent = "--- knots"; + } + } + + // armed + mode + if (armedEl) { + if (telemetry.armed) { + armedEl.textContent = "ARMED"; + armedEl.style.background = "#81c784"; + } else { + armedEl.textContent = "DISARM"; + armedEl.style.background = "#9e9e9e"; + } + } + if (modeEl) modeEl.textContent = telemetry.modeName || "—"; + } + + // Call initTelemetryDisplay() after the DOM is ready + initTelemetryDisplay(); + + // --- StatusText log + const StatusLog = { + max: 500, + items: [], // { t: Date, sev: number, txt: string } + + push(sev, txt) { + // normalise & strip trailing NULs + if (Array.isArray(txt)) txt = String.fromCharCode(...txt); + txt = String(txt || "").replace(/\0+$/, ""); + this.items.push({ t: new Date(), sev: sev ?? -1, txt }); + if (this.items.length > this.max) this.items.splice(0, this.items.length - this.max); + this.renderIfOpen(); + }, + + // severity to short tag + tag(sev) { + const map = ["EMERG","ALERT","CRIT","ERR","WARN","NOTICE","INFO","DEBUG"]; + // MAV_SEVERITY matches this order in ArduPilot (0..7) + return (sev >= 0 && sev < map.length) ? map[sev] : "INFO"; + }, + + // UI + _tip: null, + _box: null, + + open(anchorEl) { + if (this._tip) { this._tip.show(); return; } + + const wrap = document.createElement("div"); + wrap.style.cssText = "display:flex; flex-direction:column; gap:6px; width:520px;"; + + const header = document.createElement("div"); + header.textContent = "Messages (STATUSTEXT)"; + header.style.cssText = "font-weight:600;"; + + const box = document.createElement("pre"); + // ~10 rows visible. line-height ~1.25 → height ≈ 10*1.25em + padding + box.style.cssText = ` + margin:0; background:#111; color:#eee; border-radius:6px; + padding:8px; max-height:15.5em; overflow:auto; + font: 12px/1.25 ui-monospace, SFMono-Regular, Menlo, monospace; + white-space: pre-wrap; word-break: break-word; + `; + this._box = box; + + // tiny footer + const help = document.createElement("div"); + help.style.cssText = "opacity:.7; font-size:12px;"; + help.textContent = "Newest at the bottom. Keeps last 500 messages."; + + wrap.append(header, box, help); + + this._tip = tippy(anchorEl, { + content: wrap, + interactive: true, + trigger: "click", + theme: "light-border", + placement: "right-start", + appendTo: () => document.body, + onShow: () => this.renderIfOpen() + }); + + this._tip.show(); + }, + + renderIfOpen() { + if (!this._box) return; + const lines = this.items.map(it => { + // HH:MM:SS [SEV] message + const t = it.t.toTimeString().slice(0,8); + return `${t} [${this.tag(it.sev)}] ${it.txt}`; + }); + this._box.textContent = lines.join("\n"); + // autoscroll to bottom + this._box.scrollTop = this._box.scrollHeight; + } + }; + + // setup the menu + initMenuButton(); + + // Fence handling + let ftp = null; + let fenceParser = new FenceParser(); + let fenceLayers = []; + let fenceFetched = false; + let fenceRetryInterval = null; + + // Initialize FTP when connected + function initFTP() { + if (ws && ws.readyState === WebSocket.OPEN) { + ftp = new MAVFTP(MAVLink, ws); + ftp.targetSystem = vehSysId; + ftp.targetComponent = vehCompId; + console.log("FTP initialized"); + } + } + + // Start fence fetch retry loop + function startFenceFetchRetry() { + // Clear any existing interval + if (fenceRetryInterval) { + clearInterval(fenceRetryInterval); + } + + // Reset fence fetched flag + fenceFetched = false; + + // Try to fetch immediately + fetchFence(true); // true = silent mode + + // Retry every 5 seconds until successful + fenceRetryInterval = setInterval(() => { + if (!fenceFetched && ftp) { + console.log("Retrying fence fetch..."); + fetchFence(true); // silent retry + } else if (fenceFetched) { + // Stop retrying once we have the fence + clearInterval(fenceRetryInterval); + fenceRetryInterval = null; + } + }, 5000); + } + + // Stop fence fetch retry loop + function stopFenceFetchRetry() { + if (fenceRetryInterval) { + clearInterval(fenceRetryInterval); + fenceRetryInterval = null; + } + } + + // Fetch fence from vehicle + function fetchFence(silent = false) { + if (!ftp) { + if (!silent) toast("Not connected"); + return; + } + + if (!silent) toast("Fetching fence..."); + + ftp.getFile("@MISSION/fence.dat", (data) => { + if (!data) { + if (!silent) toast("Failed to fetch fence"); + return; + } + + const fences = fenceParser.parse(data); + if (fences) { + displayFences(fences); + console.log(`Loaded ${fences.length} fence items`); + if (!silent) toast(`Loaded ${fences.length} fence items`); + fenceFetched = true; // Mark as successfully fetched + stopFenceFetchRetry(); // Stop retrying + } else { + if (!silent) toast("Failed to parse fence"); + } + }); + } + + function fenceDisable() { + sendCommandInt(mavlink20.MAV_CMD_DO_FENCE_ENABLE, [ 0 ]); + } + function fenceEnable() { + sendCommandInt(mavlink20.MAV_CMD_DO_FENCE_ENABLE, [ 1 ]); + } + + // Initialize menu button + function initMenuButton() { + const toolbar = document.getElementById("toolbar"); + const recenterBtn = document.getElementById("recenterBtn"); + + // Create menu button + const menuBtn = document.createElement("button"); + menuBtn.id = "menuBtn"; + menuBtn.className = "btn small"; + menuBtn.innerHTML = "☰"; // Hamburger icon + menuBtn.style.fontSize = "18px"; + + // Insert before recenter button + toolbar.insertBefore(menuBtn, recenterBtn); + + // Create menu using Tippy + const menuDiv = document.createElement("div"); + menuDiv.style.cssText = ` + display: flex; + flex-direction: column; + gap: 4px; + padding: 4px; + min-width: 150px; + `; + + // Menu items + const menuItems = [ + { text: "Fetch Fence", action: () => { fetchFence(); menuTip.hide(); }}, + { text: "Fence Disable", action: () => { fenceDisable(); menuTip.hide(); }}, + { text: "Fence Enable", action: () => { fenceEnable(); menuTip.hide(); }}, + { text: "Messages", action: () => { StatusLog.open(menuBtn); } }, + ]; + + /* + menuItems.push( + { text: "Dev: Enable cache‑bust", action: () => { window.__dev__?.enable(true); } }, + { text: "Dev: Hard reload (cache‑bust)", action: () => { window.__dev__?.hardReload(); } }, + { text: "Dev: Disable cache‑bust", action: () => { window.__dev__?.enable(false); } }, + ); + */ + + menuItems.forEach(item => { + const btn = document.createElement("button"); + btn.style.cssText = ` + padding: 8px 12px; + background: #f0f0f0; + border: none; + border-radius: 4px; + cursor: pointer; + text-align: left; + font-size: 14px; + transition: background 0.2s; + `; + btn.textContent = item.text; + btn.onmouseover = () => btn.style.background = "#e0e0e0"; + btn.onmouseout = () => btn.style.background = "#f0f0f0"; + btn.onclick = item.action; + menuDiv.appendChild(btn); + }); + + // Create Tippy tooltip as menu + const menuTip = tippy(menuBtn, { + content: menuDiv, + interactive: true, + trigger: "click", + theme: "light-border", + placement: "right-start", + appendTo: () => document.body + }); + + return menuBtn; + } + + // Display fences on map + function displayFences(fences) { + // Clear existing fence layers + fenceLayers.forEach(layer => map.removeLayer(layer)); + fenceLayers = []; + + fences.forEach((fence, idx) => { + let layer = null; + + if (fence.type === mavlink20.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) { + // Circle inclusion (green) + layer = L.circle([fence.center.lat, fence.center.lng], { + radius: fence.radius, + color: '#4caf50', + fillColor: '#4caf50', + fillOpacity: 0, + weight: 2 + }).addTo(map); + layer.bindPopup(`Circle Inclusion #${idx}
Radius: ${fence.radius}m`); + + } else if (fence.type === mavlink20.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) { + // Circle exclusion (red) + layer = L.circle([fence.center.lat, fence.center.lng], { + radius: fence.radius, + color: '#f44336', + fillColor: '#f44336', + fillOpacity: 0, + weight: 2 + }).addTo(map); + } else if (fence.type === mavlink20.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) { + // Polygon inclusion (green) + const latlngs = fence.vertices.map(v => [v.lat, v.lng]); + layer = L.polygon(latlngs, { + color: '#4caf50', + fillColor: '#4caf50', + fillOpacity: 0, + weight: 2 + }).addTo(map); + } else if (fence.type === mavlink20.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + // Polygon exclusion (red) + const latlngs = fence.vertices.map(v => [v.lat, v.lng]); + layer = L.polygon(latlngs, { + color: '#f44336', + fillColor: '#f44336', + fillOpacity: 0, + weight: 2 + }).addTo(map); + } + + if (layer) { + fenceLayers.push(layer); + } + }); + + // Zoom to show all fences + if (fenceLayers.length > 0) { + const group = L.featureGroup(fenceLayers); + map.fitBounds(group.getBounds().pad(0.1)); + } + } + + function classifyVehicle(mavType) { + if (mavType === mavlink20.MAV_TYPE_SURFACE_BOAT) return "boat"; // MAV_TYPE_SURFACE_BOAT + if (mavType === mavlink20.MAV_TYPE_GROUND_ROVER) return "rover"; // MAV_TYPE_GROUND_ROVER + if (mavType === mavlink20.MAV_TYPE_FIXED_WING) return "plane"; // MAV_TYPE_FIXED_WING + if (mavType === mavlink20.MAV_TYPE_QUADROTOR || mavType === mavlink20.MAV_TYPE_COAXIAL || mavType === mavlink20.MAV_TYPE_HELICOPTER) return "copter"; // quad/coax/heli + return "plane"; + } + + function makeSvgIcon(kind, rotateDeg = 0) { + const paths = { + plane: ` + + + + + + + + + + + + `, + copter: ` + + + + + + + + + + + + + + `, + rover: ` + + + + + + + + + + + + + `, + boat: ` + + + + + + + + + + ` + }; + + const svg = ` + + ${paths[kind] || paths.plane} + `; + + return L.divIcon({ + html: svg, + className: "veh-ico", + iconSize: [40, 40], + iconAnchor: [20, 20] + }); + } + + function ensureMarker(lat, lon) { + if (!vehicleMarker) { + vehicleMarker = L.marker([lat, lon], { icon: makeSvgIcon(VehicleType.cls, lastHeadingDeg || 0) }).addTo(map); + appliedVehClass = VehicleType.cls; + } else { + vehicleMarker.setLatLng([lat, lon]); + } + + // Update icon if vehicle class changed or heading changed + if (appliedVehClass !== VehicleType.cls || lastHeadingDeg != null) { + vehicleMarker.setIcon(makeSvgIcon(VehicleType.cls, lastHeadingDeg || 0)); + appliedVehClass = VehicleType.cls; + } + } + + function rotateMarker(deg) { + if (!vehicleMarker) return; + + // Update the icon with new rotation + vehicleMarker.setIcon(makeSvgIcon(VehicleType.cls, deg)); + } + + function updateHeadingFromATTITUDE(yawRad) { + let deg = yawRad * 180/Math.PI; + deg = (deg + 360) % 360; + lastHeadingDeg = deg; + rotateMarker(deg); + } + + // update vehicle target position + function updateTargetPosition(lat, lon) { + if (!targetMarker) { + // Create a small red circle marker for the target + targetMarker = L.circleMarker([lat, lon], { + radius: 8, + color: '#f44336', + fillColor: '#f44336', + fillOpacity: 0.6, + weight: 2 + }).addTo(map); + targetMarker.bindPopup("Target Position"); + } else { + targetMarker.setLatLng([lat, lon]); + } + } + + // remove the target marker when not needed: + function clearTargetPosition() { + if (targetMarker) { + map.removeLayer(targetMarker); + targetMarker = null; + } + } + + // --- Connection dialog on left-bar Connect button --- + (function initConnectTip(){ + const button = connectBtn; + const tipDiv = document.createElement("div"); + tipDiv.appendChild(document.importNode(document.getElementById("connection_tip_template").content, true)); + const tip = tippy(button, { + content: tipDiv, interactive: true, trigger: "click", theme: "light-border", + appendTo: () => document.body, placement: "right-start" + }); + tipDiv.querySelector("#Close").onclick = () => tip.hide(); + + const url_input = tipDiv.querySelector("#target_url"); + const hb_checkbox = tipDiv.querySelector("#send_heartbeat"); + const passphrase_input = tipDiv.querySelector("#signing_passphrase"); + const connect_btn = tipDiv.querySelector("#connection_button"); + const disconnect_btn = tipDiv.querySelector("#disconnection_button"); + + // use random sysid and compid so multiple users are less likely to collide + const sys_input = tipDiv.querySelector("#system_id"); + const comp_input = tipDiv.querySelector("#component_id"); + // default random IDs between 100–200 inclusive + function rand100_200() { + return Math.floor(Math.random() * 101) + 100; // 0–100 + 100 → 100–200 + } + sys_input.value = rand100_200(); + comp_input.value = rand100_200(); + + const LS_KEYS = { + url: "gcs.url", + pass: "gcs.passphrase" + }; + + // preload saved values (fallbacks preserved) + url_input.value = localStorage.getItem(LS_KEYS.url) || "ws://127.0.0.1:56781"; + passphrase_input.value = localStorage.getItem(LS_KEYS.pass) || ""; + + + function applyIds() { + let sid = parseInt(sys_input.value || "255", 10); + let cid = parseInt(comp_input.value || "190", 10); + sid = (sid>=1 && sid<=255) ? sid : 255; + cid = (cid>=0 && cid<=255) ? cid : 190; + gcsSystemId = sid; gcsComponentId = cid; + + MAVLink.srcSystem = sid; + MAVLink.srcComponent = cid; + } + + function setConnState(state) { + // color feedback on the left button + if (state === "connected") button.style.background = "#00c853"; + else if (state === "connecting")button.style.background = "#f9a825"; + else if (state === "error") button.style.background = "#e53935"; + else button.style.background = ""; // default style + } + + function startHeartbeatLoop() { + if (hbInterval) { clearInterval(hbInterval); hbInterval = null; } + if (!hb_checkbox.checked) return; + hbInterval = setInterval(() => { + try { + if (!setupSigning) { + const pass = passphrase_input.value.trim(); + if (pass.length > 0) { + setupSigning = true; + const enc = new TextEncoder(); + const hash = mavlink20.sha256(enc.encode(pass)); + MAVLink.signing.secret_key = new Uint8Array(hash); + MAVLink.signing.sign_outgoing = true; + } + } + const msg = new mavlink20.messages.heartbeat( + 6, // MAV_TYPE_GCS + 8, // MAV_AUTOPILOT_INVALID + 0, 0, 4 + ); + const pkt = msg.pack(MAVLink); + ws?.send(Uint8Array.from(pkt)); + } catch (e) { + console.error("Heartbeat send failed:", e?.message || e); + if (hbInterval) { clearInterval(hbInterval); hbInterval = null; } + setConnState("error"); + toast("Heartbeat stopped after error"); + } + }, 1000); + } + + function connect(url) { + applyIds(); + if (ws) disconnect(); + setupSigning = false; + + setConnState("connecting"); + ws = new WebSocket(url); + ws.binaryType = "arraybuffer"; + + ws.onopen = () => { + tip.hide(); + setConnState("connected"); + toast("Connected"); + startHeartbeatLoop(); + initFTP(); + startFenceFetchRetry(); + }; + ws.onerror = () => { + setConnState("error"); + }; + ws.onclose = () => { + if (hbInterval) { clearInterval(hbInterval); hbInterval = null; } + setConnState(""); // default + stopFenceFetchRetry(); + toast("Disconnected"); + }; + ws.onmessage = (evt) => { + const buf = new Uint8Array(evt.data); + MAVLink.pushBuffer(buf); + while (true) { + const m = MAVLink.parseChar(null); + if (m === null) break; + if (m._id == -1) continue; + + // Learn target addresses + if (typeof m.sysid === "number") vehSysId = m.sysid; + if (typeof m.compid === "number") vehCompId = m.compid; + + // HEARTBEAT => vehicle type + if (m._name === "HEARTBEAT") { + VehicleType.mavType = m.type; + VehicleType.cls = classifyVehicle(m.type); + VehicleType.lastSeen = Date.now(); + + telemetry.armed = !!(m.base_mode & mavlink20.MAV_MODE_FLAG_SAFETY_ARMED); + // Mode name: for Rover/Boat use known names; otherwise show numeric + const isRoverish = (VehicleType.mavType === mavlink20.MAV_TYPE_GROUND_ROVER) || + (VehicleType.mavType === mavlink20.MAV_TYPE_SURFACE_BOAT); + telemetry.modeName = isRoverish ? (roverModeNames[m.custom_mode] || `${m.custom_mode}`) : `${m.custom_mode}`; + + updateTelemetryDisplay(); + } + + // GLOBAL_POSITION_INT => lat/lon + heading if present and speed + if (m._name === "GLOBAL_POSITION_INT") { + const lat = m.lat / 1e7, lon = m.lon / 1e7; + ensureMarker(lat, lon); + if (!map._movedOnce) { map.setView([lat, lon], 16); map._movedOnce = true; } + + // Calculate ground speed from vx and vy (cm/s) + const vx_ms = m.vx / 100.0; // Convert cm/s to m/s + const vy_ms = m.vy / 100.0; + telemetry.speed = Math.sqrt(vx_ms * vx_ms + vy_ms * vy_ms); + updateTelemetryDisplay(); + } + + // ATTITUDE => yaw (continuous) + if (m._name === "ATTITUDE") { + if (typeof m.yaw === "number") updateHeadingFromATTITUDE(m.yaw); + } + + // Handle FTP messages + if (ftp && m._name === "FILE_TRANSFER_PROTOCOL") { + ftp.handleMessage(m); + } + + // BATTERY_STATUS => battery percentage + if (m._name === "BATTERY_STATUS") { + telemetry.batteryPct = m.battery_remaining; + telemetry.lastUpdate = Date.now(); + telemetry.currentA = m.current_battery / 100.0; + updateTelemetryDisplay(); + } + + if (m._name === "POSITION_TARGET_GLOBAL_INT") { + if (m.lat_int !== 0 || m.lon_int !== 0) { + const lat = m.lat_int / 1e7; + const lon = m.lon_int / 1e7; + updateTargetPosition(lat, lon); + lastTargetSeenMs = Date.now(); + } else { + clearTargetPosition(); + lastTargetSeenMs = 0; + } + } + + // STATUSTEXT / STATUSTEXT_LONG => capture into Messages log + if (m._name === "STATUSTEXT") { + StatusLog.push(m.severity, m.text); + } + }; + } + } + + function disconnect() { + if (ws) { try { ws.close(); } catch {} } + ws = null; + if (hbInterval) { clearInterval(hbInterval); hbInterval = null; } + setConnState(""); + clearTargetPosition(); + } + + connect_btn.onclick = () => { + if (!url_input.checkValidity()) { toast("Enter ws:// or wss:// URL"); url_input.focus(); return; } + + // remember URL and passphrase + localStorage.setItem(LS_KEYS.url, url_input.value.trim()); + const pass = passphrase_input.value.trim(); + if (pass.length) { + localStorage.setItem(LS_KEYS.pass, pass); + } else { + localStorage.removeItem(LS_KEYS.pass); // don't store empty + } + + connect(url_input.value); + }; + disconnect_btn.onclick = () => disconnect(); + })(); + + // Buttons + armBtn.onclick = () => { sendCommandInt(mavlink20.MAV_CMD_COMPONENT_ARM_DISARM, [1]); toast("ARM sent"); }; // MAV_CMD_COMPONENT_ARM_DISARM + disarmBtn.onclick = () => { sendCommandInt(mavlink20.MAV_CMD_COMPONENT_ARM_DISARM, [0]); toast("DISARM sent"); }; + rtlBtn.onclick = () => { sendSetMode(roverModes.RTL, []); toast("RTL sent"); }; + loiterBtn.onclick = () => { sendSetMode(roverModes.LOITER, []); toast("LOITER sent"); }; + recenterBtn.onclick = () => { + if (vehicleMarker) { + const ll = vehicleMarker.getLatLng(); + map.setView(ll, Math.max(map.getZoom(), 16)); + } + }; + + // --- Long-press to DO_REPOSITION (desktop + mobile), avoids pan-as-click --- + (() => { + const el = map.getContainer(); + let pressTimer = null, startPt = null, lastPt = null, activeId = null; + const HOLD_MS = 600, MOVE_PX_TOL = 10; + + function clearAll(){ if (pressTimer){ clearTimeout(pressTimer); pressTimer=null; } activeId=null; startPt=lastPt=null; } + + el.addEventListener("pointerdown", (ev) => { + if (ev.target.closest(".leaflet-control")) return; + if (ev.pointerType === "touch") ev.preventDefault(); // <- suppress iOS long‑press callout + + activeId = ev.pointerId; + startPt = lastPt = map.mouseEventToContainerPoint(ev); + pressTimer = setTimeout(() => { + if (!lastPt) return; + const ll = map.containerPointToLatLng(lastPt); + sendCommandInt(mavlink20.MAV_CMD_DO_REPOSITION, [ + 0, mavlink20.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, 0, 0, + ll.lat*1e7, ll.lng*1e7, 0 + ]); + toast("DO_REPOSITION sent"); + clearAll(); + }, HOLD_MS); + }, { passive: false }); + + el.addEventListener("pointermove", (ev) => { + if (ev.pointerId !== activeId) return; + if (ev.pointerType === "touch") ev.preventDefault(); // keep iOS from starting callout/scroll + lastPt = map.mouseEventToContainerPoint(ev); + if (startPt && lastPt && startPt.distanceTo(lastPt) > MOVE_PX_TOL) clearAll(); + }, { passive: false }); + + ["pointerup","pointercancel","pointerleave","pointerout"].forEach(t => + el.addEventListener(t, (ev) => { if (ev.pointerId === activeId) clearAll(); }, { passive:false }) + ); + })(); + + // clear target if stale + setInterval(() => { + if (targetMarker && lastTargetSeenMs && (Date.now() - lastTargetSeenMs > 5000)) { + clearTargetPosition(); + lastTargetSeenMs = 0; + } + }, 1000); + + console.log("Simple GCS Map ready."); +})(); diff --git a/SimpleGCS/gcs_icon_32.png b/SimpleGCS/gcs_icon_32.png new file mode 100644 index 00000000..cc136872 Binary files /dev/null and b/SimpleGCS/gcs_icon_32.png differ diff --git a/SimpleGCS/index.html b/SimpleGCS/index.html new file mode 100644 index 00000000..f59c68ce --- /dev/null +++ b/SimpleGCS/index.html @@ -0,0 +1,150 @@ + + + + + + Simple GCS Map + + + + + + + + + + + + + + + + + + + +
+ +
+
+ + + + + + + diff --git a/TelemetryDashboard/Examples/URDF_Viewer.json b/TelemetryDashboard/Examples/URDF_Viewer.json index a4e8af4f..3f7d4705 100644 --- a/TelemetryDashboard/Examples/URDF_Viewer.json +++ b/TelemetryDashboard/Examples/URDF_Viewer.json @@ -277,7 +277,7 @@ "name": "URDF Viewer example", "info": "URDF viewer example giving 3D aircraft image and animation using the custom HTML widget" }, - "custom_HTML": "\n\n\n\n \n Sandbox\n \n \n\n\n\n\n\n" + "custom_HTML": "\n\n\n\n \n Sandbox\n \n \n\n\n\n\n\n" } } -} \ No newline at end of file +} diff --git a/TelemetryDashboard/MAVLink/local_modules/long/src/long.js b/TelemetryDashboard/MAVLink/local_modules/long/src/long.js deleted file mode 100644 index 6eec276c..00000000 --- a/TelemetryDashboard/MAVLink/local_modules/long/src/long.js +++ /dev/null @@ -1,1325 +0,0 @@ -// This file is MODIFIED from the original, by buzz 2020, please see README.md in the upper level folder for more details. - -module.exports = Long; - -/** - * wasm optimizations, to do native i64 multiplication and divide - */ -var wasm = null; - -try { - wasm = new WebAssembly.Instance(new WebAssembly.Module(new Uint8Array([ - 0, 97, 115, 109, 1, 0, 0, 0, 1, 13, 2, 96, 0, 1, 127, 96, 4, 127, 127, 127, 127, 1, 127, 3, 7, 6, 0, 1, 1, 1, 1, 1, 6, 6, 1, 127, 1, 65, 0, 11, 7, 50, 6, 3, 109, 117, 108, 0, 1, 5, 100, 105, 118, 95, 115, 0, 2, 5, 100, 105, 118, 95, 117, 0, 3, 5, 114, 101, 109, 95, 115, 0, 4, 5, 114, 101, 109, 95, 117, 0, 5, 8, 103, 101, 116, 95, 104, 105, 103, 104, 0, 0, 10, 191, 1, 6, 4, 0, 35, 0, 11, 36, 1, 1, 126, 32, 0, 173, 32, 1, 173, 66, 32, 134, 132, 32, 2, 173, 32, 3, 173, 66, 32, 134, 132, 126, 34, 4, 66, 32, 135, 167, 36, 0, 32, 4, 167, 11, 36, 1, 1, 126, 32, 0, 173, 32, 1, 173, 66, 32, 134, 132, 32, 2, 173, 32, 3, 173, 66, 32, 134, 132, 127, 34, 4, 66, 32, 135, 167, 36, 0, 32, 4, 167, 11, 36, 1, 1, 126, 32, 0, 173, 32, 1, 173, 66, 32, 134, 132, 32, 2, 173, 32, 3, 173, 66, 32, 134, 132, 128, 34, 4, 66, 32, 135, 167, 36, 0, 32, 4, 167, 11, 36, 1, 1, 126, 32, 0, 173, 32, 1, 173, 66, 32, 134, 132, 32, 2, 173, 32, 3, 173, 66, 32, 134, 132, 129, 34, 4, 66, 32, 135, 167, 36, 0, 32, 4, 167, 11, 36, 1, 1, 126, 32, 0, 173, 32, 1, 173, 66, 32, 134, 132, 32, 2, 173, 32, 3, 173, 66, 32, 134, 132, 130, 34, 4, 66, 32, 135, 167, 36, 0, 32, 4, 167, 11 - ])), {}).exports; -} catch (e) { - // no wasm support :( -} - -/** - * Constructs a 64 bit two's-complement integer, given its low and high 32 bit values as *signed* integers. - * See the from* functions below for more convenient ways of constructing Longs. - * @exports Long - * @class A Long class for representing a 64 bit two's-complement integer value. - * @param {number} low The low (signed) 32 bits of the long - * @param {number} high The high (signed) 32 bits of the long - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @constructor - */ -function Long(low, high, unsigned) { - - /** - * The low 32 bits as a signed value. - * @type {number} - */ - this.low = low | 0; - - /** - * The high 32 bits as a signed value. - * @type {number} - */ - this.high = high | 0; - - /** - * Whether unsigned or not. - * @type {boolean} - */ - this.unsigned = !!unsigned; -} - -// The internal representation of a long is the two given signed, 32-bit values. -// We use 32-bit pieces because these are the size of integers on which -// Javascript performs bit-operations. For operations like addition and -// multiplication, we split each number into 16 bit pieces, which can easily be -// multiplied within Javascript's floating-point representation without overflow -// or change in sign. -// -// In the algorithms below, we frequently reduce the negative case to the -// positive case by negating the input(s) and then post-processing the result. -// Note that we must ALWAYS check specially whether those values are MIN_VALUE -// (-2^63) because -MIN_VALUE == MIN_VALUE (since 2^63 cannot be represented as -// a positive number, it overflows back into a negative). Not handling this -// case would often result in infinite recursion. -// -// Common constant values ZERO, ONE, NEG_ONE, etc. are defined below the from* -// methods on which they depend. - -/** - * An indicator used to reliably determine if an object is a Long or not. - * @type {boolean} - * @const - * @private - */ -Long.prototype.__isLong__; - -Object.defineProperty(Long.prototype, "__isLong__", { value: true }); - -/** - * @function - * @param {*} obj Object - * @returns {boolean} - * @inner - */ -function isLong(obj) { - return (obj && obj["__isLong__"]) === true; -} - -/** - * Tests if the specified object is a Long. - * @function - * @param {*} obj Object - * @returns {boolean} - */ -Long.isLong = isLong; - -/** - * A cache of the Long representations of small integer values. - * @type {!Object} - * @inner - */ -var INT_CACHE = {}; - -/** - * A cache of the Long representations of small unsigned integer values. - * @type {!Object} - * @inner - */ -var UINT_CACHE = {}; - -/** - * @param {number} value - * @param {boolean=} unsigned - * @returns {!Long} - * @inner - */ -function fromInt(value, unsigned) { - var obj, cachedObj, cache; - if (unsigned) { - value >>>= 0; - if (cache = (0 <= value && value < 256)) { - cachedObj = UINT_CACHE[value]; - if (cachedObj) - return cachedObj; - } - obj = fromBits(value, (value | 0) < 0 ? -1 : 0, true); - if (cache) - UINT_CACHE[value] = obj; - return obj; - } else { - value |= 0; - if (cache = (-128 <= value && value < 128)) { - cachedObj = INT_CACHE[value]; - if (cachedObj) - return cachedObj; - } - obj = fromBits(value, value < 0 ? -1 : 0, false); - if (cache) - INT_CACHE[value] = obj; - return obj; - } -} - -/** - * Returns a Long representing the given 32 bit integer value. - * @function - * @param {number} value The 32 bit integer in question - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {!Long} The corresponding Long value - */ -Long.fromInt = fromInt; - -/** - * @param {number} value - * @param {boolean=} unsigned - * @returns {!Long} - * @inner - */ -function fromNumber(value, unsigned) { - if (isNaN(value)) - return unsigned ? UZERO : ZERO; - if (unsigned) { - if (value < 0) - return UZERO; - if (value >= TWO_PWR_64_DBL) - return MAX_UNSIGNED_VALUE; - } else { - if (value <= -TWO_PWR_63_DBL) - return MIN_VALUE; - if (value + 1 >= TWO_PWR_63_DBL) - return MAX_VALUE; - } - if (value < 0) - return fromNumber(-value, unsigned).neg(); - return fromBits((value % TWO_PWR_32_DBL) | 0, (value / TWO_PWR_32_DBL) | 0, unsigned); -} - -/** - * Returns a Long representing the given value, provided that it is a finite number. Otherwise, zero is returned. - * @function - * @param {number} value The number in question - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {!Long} The corresponding Long value - */ -Long.fromNumber = fromNumber; - -/** - * @param {number} lowBits - * @param {number} highBits - * @param {boolean=} unsigned - * @returns {!Long} - * @inner - */ -function fromBits(lowBits, highBits, unsigned) { - return new Long(lowBits, highBits, unsigned); -} - -/** - * Returns a Long representing the 64 bit integer that comes by concatenating the given low and high bits. Each is - * assumed to use 32 bits. - * @function - * @param {number} lowBits The low 32 bits - * @param {number} highBits The high 32 bits - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {!Long} The corresponding Long value - */ -Long.fromBits = fromBits; - -/** - * @function - * @param {number} base - * @param {number} exponent - * @returns {number} - * @inner - */ -var pow_dbl = Math.pow; // Used 4 times (4*8 to 15+4) - -/** - * @param {string} str - * @param {(boolean|number)=} unsigned - * @param {number=} radix - * @returns {!Long} - * @inner - */ -function fromString(str, unsigned, radix) { - if (str.length === 0) - throw Error('empty string'); - if (str === "NaN" || str === "Infinity" || str === "+Infinity" || str === "-Infinity") - return ZERO; - if (typeof unsigned === 'number') { - // For goog.math.long compatibility - radix = unsigned, - unsigned = false; - } else { - unsigned = !! unsigned; - } - radix = radix || 10; - if (radix < 2 || 36 < radix) - throw RangeError('radix'); - - var p; - if ((p = str.indexOf('-')) > 0) - throw Error('interior hyphen'); - else if (p === 0) { - return fromString(str.substring(1), unsigned, radix).neg(); - } - - // Do several (8) digits each time through the loop, so as to - // minimize the calls to the very expensive emulated div. - var radixToPower = fromNumber(pow_dbl(radix, 8)); - - var result = ZERO; - for (var i = 0; i < str.length; i += 8) { - var size = Math.min(8, str.length - i), - value = parseInt(str.substring(i, i + size), radix); - if (size < 8) { - var power = fromNumber(pow_dbl(radix, size)); - result = result.mul(power).add(fromNumber(value)); - } else { - result = result.mul(radixToPower); - result = result.add(fromNumber(value)); - } - } - result.unsigned = unsigned; - return result; -} - -/** - * Returns a Long representation of the given string, written using the specified radix. - * @function - * @param {string} str The textual representation of the Long - * @param {(boolean|number)=} unsigned Whether unsigned or not, defaults to signed - * @param {number=} radix The radix in which the text is written (2-36), defaults to 10 - * @returns {!Long} The corresponding Long value - */ -Long.fromString = fromString; - -/** - * @function - * @param {!Long|number|string|!{low: number, high: number, unsigned: boolean}} val - * @param {boolean=} unsigned - * @returns {!Long} - * @inner - */ -function fromValue(val, unsigned) { - if (typeof val === 'number') - return fromNumber(val, unsigned); - if (typeof val === 'string') - return fromString(val, unsigned); - // Throws for non-objects, converts non-instanceof Long: - return fromBits(val.low, val.high, typeof unsigned === 'boolean' ? unsigned : val.unsigned); -} - -/** - * Converts the specified value to a Long using the appropriate from* function for its type. - * @function - * @param {!Long|number|string|!{low: number, high: number, unsigned: boolean}} val Value - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {!Long} - */ -Long.fromValue = fromValue; - -// NOTE: the compiler should inline these constant values below and then remove these variables, so there should be -// no runtime penalty for these. - -/** - * @type {number} - * @const - * @inner - */ -var TWO_PWR_16_DBL = 1 << 16; - -/** - * @type {number} - * @const - * @inner - */ -var TWO_PWR_24_DBL = 1 << 24; - -/** - * @type {number} - * @const - * @inner - */ -var TWO_PWR_32_DBL = TWO_PWR_16_DBL * TWO_PWR_16_DBL; - -/** - * @type {number} - * @const - * @inner - */ -var TWO_PWR_64_DBL = TWO_PWR_32_DBL * TWO_PWR_32_DBL; - -/** - * @type {number} - * @const - * @inner - */ -var TWO_PWR_63_DBL = TWO_PWR_64_DBL / 2; - -/** - * @type {!Long} - * @const - * @inner - */ -var TWO_PWR_24 = fromInt(TWO_PWR_24_DBL); - -/** - * @type {!Long} - * @inner - */ -var ZERO = fromInt(0); - -/** - * Signed zero. - * @type {!Long} - */ -Long.ZERO = ZERO; - -/** - * @type {!Long} - * @inner - */ -var UZERO = fromInt(0, true); - -/** - * Unsigned zero. - * @type {!Long} - */ -Long.UZERO = UZERO; - -/** - * @type {!Long} - * @inner - */ -var ONE = fromInt(1); - -/** - * Signed one. - * @type {!Long} - */ -Long.ONE = ONE; - -/** - * @type {!Long} - * @inner - */ -var UONE = fromInt(1, true); - -/** - * Unsigned one. - * @type {!Long} - */ -Long.UONE = UONE; - -/** - * @type {!Long} - * @inner - */ -var NEG_ONE = fromInt(-1); - -/** - * Signed negative one. - * @type {!Long} - */ -Long.NEG_ONE = NEG_ONE; - -/** - * @type {!Long} - * @inner - */ -var MAX_VALUE = fromBits(0xFFFFFFFF|0, 0x7FFFFFFF|0, false); - -/** - * Maximum signed value. - * @type {!Long} - */ -Long.MAX_VALUE = MAX_VALUE; - -/** - * @type {!Long} - * @inner - */ -var MAX_UNSIGNED_VALUE = fromBits(0xFFFFFFFF|0, 0xFFFFFFFF|0, true); - -/** - * Maximum unsigned value. - * @type {!Long} - */ -Long.MAX_UNSIGNED_VALUE = MAX_UNSIGNED_VALUE; - -/** - * @type {!Long} - * @inner - */ -var MIN_VALUE = fromBits(0, 0x80000000|0, false); - -/** - * Minimum signed value. - * @type {!Long} - */ -Long.MIN_VALUE = MIN_VALUE; - -/** - * @alias Long.prototype - * @inner - */ -var LongPrototype = Long.prototype; - -/** - * Converts the Long to a 32 bit integer, assuming it is a 32 bit integer. - * @returns {number} - */ -LongPrototype.toInt = function toInt() { - return this.unsigned ? this.low >>> 0 : this.low; -}; - -/** - * Converts the Long to a the nearest floating-point representation of this value (double, 53 bit mantissa). - * @returns {number} - */ -LongPrototype.toNumber = function toNumber() { - if (this.unsigned) - return ((this.high >>> 0) * TWO_PWR_32_DBL) + (this.low >>> 0); - return this.high * TWO_PWR_32_DBL + (this.low >>> 0); -}; - -/** - * Converts the Long to a string written in the specified radix. - * @param {number=} radix Radix (2-36), defaults to 10 - * @returns {string} - * @override - * @throws {RangeError} If `radix` is out of range - */ -LongPrototype.toString = function toString(radix) { - radix = radix || 10; - if (radix < 2 || 36 < radix) - throw RangeError('radix'); - if (this.isZero()) - return '0'; - if (this.isNegative()) { // Unsigned Longs are never negative - if (this.eq(MIN_VALUE)) { - // We need to change the Long value before it can be negated, so we remove - // the bottom-most digit in this base and then recurse to do the rest. - var radixLong = fromNumber(radix), - div = this.div(radixLong), - rem1 = div.mul(radixLong).sub(this); - return div.toString(radix) + rem1.toInt().toString(radix); - } else - return '-' + this.neg().toString(radix); - } - - // Do several (6) digits each time through the loop, so as to - // minimize the calls to the very expensive emulated div. - var radixToPower = fromNumber(pow_dbl(radix, 6), this.unsigned), - rem = this; - var result = ''; - while (true) { - var remDiv = rem.div(radixToPower), - intval = rem.sub(remDiv.mul(radixToPower)).toInt() >>> 0, - digits = intval.toString(radix); - rem = remDiv; - if (rem.isZero()) - return digits + result; - else { - while (digits.length < 6) - digits = '0' + digits; - result = '' + digits + result; - } - } -}; - -/** - * Gets the high 32 bits as a signed integer. - * @returns {number} Signed high bits - */ -LongPrototype.getHighBits = function getHighBits() { - return this.high; -}; - -/** - * Gets the high 32 bits as an unsigned integer. - * @returns {number} Unsigned high bits - */ -LongPrototype.getHighBitsUnsigned = function getHighBitsUnsigned() { - return this.high >>> 0; -}; - -/** - * Gets the low 32 bits as a signed integer. - * @returns {number} Signed low bits - */ -LongPrototype.getLowBits = function getLowBits() { - return this.low; -}; - -/** - * Gets the low 32 bits as an unsigned integer. - * @returns {number} Unsigned low bits - */ -LongPrototype.getLowBitsUnsigned = function getLowBitsUnsigned() { - return this.low >>> 0; -}; - -/** - * Gets the number of bits needed to represent the absolute value of this Long. - * @returns {number} - */ -LongPrototype.getNumBitsAbs = function getNumBitsAbs() { - if (this.isNegative()) // Unsigned Longs are never negative - return this.eq(MIN_VALUE) ? 64 : this.neg().getNumBitsAbs(); - var val = this.high != 0 ? this.high : this.low; - for (var bit = 31; bit > 0; bit--) - if ((val & (1 << bit)) != 0) - break; - return this.high != 0 ? bit + 33 : bit + 1; -}; - -/** - * Tests if this Long's value equals zero. - * @returns {boolean} - */ -LongPrototype.isZero = function isZero() { - return this.high === 0 && this.low === 0; -}; - -/** - * Tests if this Long's value equals zero. This is an alias of {@link Long#isZero}. - * @returns {boolean} - */ -LongPrototype.eqz = LongPrototype.isZero; - -/** - * Tests if this Long's value is negative. - * @returns {boolean} - */ -LongPrototype.isNegative = function isNegative() { - return !this.unsigned && this.high < 0; -}; - -/** - * Tests if this Long's value is positive. - * @returns {boolean} - */ -LongPrototype.isPositive = function isPositive() { - return this.unsigned || this.high >= 0; -}; - -/** - * Tests if this Long's value is odd. - * @returns {boolean} - */ -LongPrototype.isOdd = function isOdd() { - return (this.low & 1) === 1; -}; - -/** - * Tests if this Long's value is even. - * @returns {boolean} - */ -LongPrototype.isEven = function isEven() { - return (this.low & 1) === 0; -}; - -/** - * Tests if this Long's value equals the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.equals = function equals(other) { - if (!isLong(other)) - other = fromValue(other); - if (this.unsigned !== other.unsigned && (this.high >>> 31) === 1 && (other.high >>> 31) === 1) - return false; - return this.high === other.high && this.low === other.low; -}; - -/** - * Tests if this Long's value equals the specified's. This is an alias of {@link Long#equals}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.eq = LongPrototype.equals; - -/** - * Tests if this Long's value differs from the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.notEquals = function notEquals(other) { - return !this.eq(/* validates */ other); -}; - -/** - * Tests if this Long's value differs from the specified's. This is an alias of {@link Long#notEquals}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.neq = LongPrototype.notEquals; - -/** - * Tests if this Long's value differs from the specified's. This is an alias of {@link Long#notEquals}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.ne = LongPrototype.notEquals; - -/** - * Tests if this Long's value is less than the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.lessThan = function lessThan(other) { - return this.comp(/* validates */ other) < 0; -}; - -/** - * Tests if this Long's value is less than the specified's. This is an alias of {@link Long#lessThan}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.lt = LongPrototype.lessThan; - -/** - * Tests if this Long's value is less than or equal the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.lessThanOrEqual = function lessThanOrEqual(other) { - return this.comp(/* validates */ other) <= 0; -}; - -/** - * Tests if this Long's value is less than or equal the specified's. This is an alias of {@link Long#lessThanOrEqual}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.lte = LongPrototype.lessThanOrEqual; - -/** - * Tests if this Long's value is less than or equal the specified's. This is an alias of {@link Long#lessThanOrEqual}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.le = LongPrototype.lessThanOrEqual; - -/** - * Tests if this Long's value is greater than the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.greaterThan = function greaterThan(other) { - return this.comp(/* validates */ other) > 0; -}; - -/** - * Tests if this Long's value is greater than the specified's. This is an alias of {@link Long#greaterThan}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.gt = LongPrototype.greaterThan; - -/** - * Tests if this Long's value is greater than or equal the specified's. - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.greaterThanOrEqual = function greaterThanOrEqual(other) { - return this.comp(/* validates */ other) >= 0; -}; - -/** - * Tests if this Long's value is greater than or equal the specified's. This is an alias of {@link Long#greaterThanOrEqual}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.gte = LongPrototype.greaterThanOrEqual; - -/** - * Tests if this Long's value is greater than or equal the specified's. This is an alias of {@link Long#greaterThanOrEqual}. - * @function - * @param {!Long|number|string} other Other value - * @returns {boolean} - */ -LongPrototype.ge = LongPrototype.greaterThanOrEqual; - -/** - * Compares this Long's value with the specified's. - * @param {!Long|number|string} other Other value - * @returns {number} 0 if they are the same, 1 if the this is greater and -1 - * if the given one is greater - */ -LongPrototype.compare = function compare(other) { - if (!isLong(other)) - other = fromValue(other); - if (this.eq(other)) - return 0; - var thisNeg = this.isNegative(), - otherNeg = other.isNegative(); - if (thisNeg && !otherNeg) - return -1; - if (!thisNeg && otherNeg) - return 1; - // At this point the sign bits are the same - if (!this.unsigned) - return this.sub(other).isNegative() ? -1 : 1; - // Both are positive if at least one is unsigned - return (other.high >>> 0) > (this.high >>> 0) || (other.high === this.high && (other.low >>> 0) > (this.low >>> 0)) ? -1 : 1; -}; - -/** - * Compares this Long's value with the specified's. This is an alias of {@link Long#compare}. - * @function - * @param {!Long|number|string} other Other value - * @returns {number} 0 if they are the same, 1 if the this is greater and -1 - * if the given one is greater - */ -LongPrototype.comp = LongPrototype.compare; - -/** - * Negates this Long's value. - * @returns {!Long} Negated Long - */ -LongPrototype.negate = function negate() { - if (!this.unsigned && this.eq(MIN_VALUE)) - return MIN_VALUE; - return this.not().add(ONE); -}; - -/** - * Negates this Long's value. This is an alias of {@link Long#negate}. - * @function - * @returns {!Long} Negated Long - */ -LongPrototype.neg = LongPrototype.negate; - -/** - * Returns the sum of this and the specified Long. - * @param {!Long|number|string} addend Addend - * @returns {!Long} Sum - */ -LongPrototype.add = function add(addend) { - if (!isLong(addend)) - addend = fromValue(addend); - - // Divide each number into 4 chunks of 16 bits, and then sum the chunks. - - var a48 = this.high >>> 16; - var a32 = this.high & 0xFFFF; - var a16 = this.low >>> 16; - var a00 = this.low & 0xFFFF; - - var b48 = addend.high >>> 16; - var b32 = addend.high & 0xFFFF; - var b16 = addend.low >>> 16; - var b00 = addend.low & 0xFFFF; - - var c48 = 0, c32 = 0, c16 = 0, c00 = 0; - c00 += a00 + b00; - c16 += c00 >>> 16; - c00 &= 0xFFFF; - c16 += a16 + b16; - c32 += c16 >>> 16; - c16 &= 0xFFFF; - c32 += a32 + b32; - c48 += c32 >>> 16; - c32 &= 0xFFFF; - c48 += a48 + b48; - c48 &= 0xFFFF; - return fromBits((c16 << 16) | c00, (c48 << 16) | c32, this.unsigned); -}; - -/** - * Returns the difference of this and the specified Long. - * @param {!Long|number|string} subtrahend Subtrahend - * @returns {!Long} Difference - */ -LongPrototype.subtract = function subtract(subtrahend) { - if (!isLong(subtrahend)) - subtrahend = fromValue(subtrahend); - return this.add(subtrahend.neg()); -}; - -/** - * Returns the difference of this and the specified Long. This is an alias of {@link Long#subtract}. - * @function - * @param {!Long|number|string} subtrahend Subtrahend - * @returns {!Long} Difference - */ -LongPrototype.sub = LongPrototype.subtract; - -/** - * Returns the product of this and the specified Long. - * @param {!Long|number|string} multiplier Multiplier - * @returns {!Long} Product - */ -LongPrototype.multiply = function multiply(multiplier) { - if (this.isZero()) - return this; - if (!isLong(multiplier)) - multiplier = fromValue(multiplier); - - // use wasm support if present - if (wasm) { - var low = wasm.mul(this.low, - this.high, - multiplier.low, - multiplier.high); - return fromBits(low, wasm.get_high(), this.unsigned); - } - - if (multiplier.isZero()) - return (this.unsigned?UZERO:ZERO); - if (this.eq(MIN_VALUE)) - return multiplier.isOdd() ? MIN_VALUE : (this.unsigned?UZERO:ZERO); - if (multiplier.eq(MIN_VALUE)) - return this.isOdd() ? MIN_VALUE : (this.unsigned?UZERO:ZERO); - - if (this.isNegative()) { - if (multiplier.isNegative()) - return this.neg().mul(multiplier.neg()); - else - return this.neg().mul(multiplier).neg(); - } else if (multiplier.isNegative()) - return this.mul(multiplier.neg()).neg(); - - // If both longs are small, use float multiplication - if (this.lt(TWO_PWR_24) && multiplier.lt(TWO_PWR_24)) - return fromNumber(this.toNumber() * multiplier.toNumber(), this.unsigned); - - // Divide each long into 4 chunks of 16 bits, and then add up 4x4 products. - // We can skip products that would overflow. - - var a48 = this.high >>> 16; - var a32 = this.high & 0xFFFF; - var a16 = this.low >>> 16; - var a00 = this.low & 0xFFFF; - - var b48 = multiplier.high >>> 16; - var b32 = multiplier.high & 0xFFFF; - var b16 = multiplier.low >>> 16; - var b00 = multiplier.low & 0xFFFF; - - var c48 = 0, c32 = 0, c16 = 0, c00 = 0; - c00 += a00 * b00; - c16 += c00 >>> 16; - c00 &= 0xFFFF; - c16 += a16 * b00; - c32 += c16 >>> 16; - c16 &= 0xFFFF; - c16 += a00 * b16; - c32 += c16 >>> 16; - c16 &= 0xFFFF; - c32 += a32 * b00; - c48 += c32 >>> 16; - c32 &= 0xFFFF; - c32 += a16 * b16; - c48 += c32 >>> 16; - c32 &= 0xFFFF; - c32 += a00 * b32; - c48 += c32 >>> 16; - c32 &= 0xFFFF; - c48 += a48 * b00 + a32 * b16 + a16 * b32 + a00 * b48; - c48 &= 0xFFFF; - return fromBits((c16 << 16) | c00, (c48 << 16) | c32, this.unsigned); -}; - -/** - * Returns the product of this and the specified Long. This is an alias of {@link Long#multiply}. - * @function - * @param {!Long|number|string} multiplier Multiplier - * @returns {!Long} Product - */ -LongPrototype.mul = LongPrototype.multiply; - -/** - * Returns this Long divided by the specified. The result is signed if this Long is signed or - * unsigned if this Long is unsigned. - * @param {!Long|number|string} divisor Divisor - * @returns {!Long} Quotient - */ -LongPrototype.divide = function divide(divisor) { - if (!isLong(divisor)) - divisor = fromValue(divisor); - if (divisor.isZero()) - throw Error('division by zero'); - - // use wasm support if present - if (wasm) { - // guard against signed division overflow: the largest - // negative number / -1 would be 1 larger than the largest - // positive number, due to two's complement. - if (!this.unsigned && - this.high === -0x80000000 && - divisor.low === -1 && divisor.high === -1) { - // be consistent with non-wasm code path - return this; - } - var low = (this.unsigned ? wasm.div_u : wasm.div_s)( - this.low, - this.high, - divisor.low, - divisor.high - ); - return fromBits(low, wasm.get_high(), this.unsigned); - } - - if (this.isZero()) - return this.unsigned ? UZERO : ZERO; - var approx, rem, res; - if (!this.unsigned) { - // This section is only relevant for signed longs and is derived from the - // closure library as a whole. - if (this.eq(MIN_VALUE)) { - if (divisor.eq(ONE) || divisor.eq(NEG_ONE)) - return MIN_VALUE; // recall that -MIN_VALUE == MIN_VALUE - else if (divisor.eq(MIN_VALUE)) - return ONE; - else { - // At this point, we have |other| >= 2, so |this/other| < |MIN_VALUE|. - var halfThis = this.shr(1); - approx = halfThis.div(divisor).shl(1); - if (approx.eq(ZERO)) { - return divisor.isNegative() ? ONE : NEG_ONE; - } else { - rem = this.sub(divisor.mul(approx)); - res = approx.add(rem.div(divisor)); - return res; - } - } - } else if (divisor.eq(MIN_VALUE)) - return this.unsigned ? UZERO : ZERO; - if (this.isNegative()) { - if (divisor.isNegative()) - return this.neg().div(divisor.neg()); - return this.neg().div(divisor).neg(); - } else if (divisor.isNegative()) - return this.div(divisor.neg()).neg(); - res = ZERO; - } else { - // The algorithm below has not been made for unsigned longs. It's therefore - // required to take special care of the MSB prior to running it. - if (!divisor.unsigned) - divisor = divisor.toUnsigned(); - if (divisor.gt(this)) - return UZERO; - if (divisor.gt(this.shru(1))) // 15 >>> 1 = 7 ; with divisor = 8 ; true - return UONE; - res = UZERO; - } - - // Repeat the following until the remainder is less than other: find a - // floating-point that approximates remainder / other *from below*, add this - // into the result, and subtract it from the remainder. It is critical that - // the approximate value is less than or equal to the real value so that the - // remainder never becomes negative. - rem = this; - while (rem.gte(divisor)) { - // Approximate the result of division. This may be a little greater or - // smaller than the actual value. - approx = Math.max(1, Math.floor(rem.toNumber() / divisor.toNumber())); - - // We will tweak the approximate result by changing it in the 48-th digit or - // the smallest non-fractional digit, whichever is larger. - var log2 = Math.ceil(Math.log(approx) / Math.LN2), - delta = (log2 <= 48) ? 1 : pow_dbl(2, log2 - 48), - - // Decrease the approximation until it is smaller than the remainder. Note - // that if it is too large, the product overflows and is negative. - approxRes = fromNumber(approx), - approxRem = approxRes.mul(divisor); - while (approxRem.isNegative() || approxRem.gt(rem)) { - approx -= delta; - approxRes = fromNumber(approx, this.unsigned); - approxRem = approxRes.mul(divisor); - } - - // We know the answer can't be zero... and actually, zero would cause - // infinite recursion since we would make no progress. - if (approxRes.isZero()) - approxRes = ONE; - - res = res.add(approxRes); - rem = rem.sub(approxRem); - } - return res; -}; - -/** - * Returns this Long divided by the specified. This is an alias of {@link Long#divide}. - * @function - * @param {!Long|number|string} divisor Divisor - * @returns {!Long} Quotient - */ -LongPrototype.div = LongPrototype.divide; - -/** - * Returns this Long modulo the specified. - * @param {!Long|number|string} divisor Divisor - * @returns {!Long} Remainder - */ -LongPrototype.modulo = function modulo(divisor) { - if (!isLong(divisor)) - divisor = fromValue(divisor); - - // use wasm support if present - if (wasm) { - var low = (this.unsigned ? wasm.rem_u : wasm.rem_s)( - this.low, - this.high, - divisor.low, - divisor.high - ); - return fromBits(low, wasm.get_high(), this.unsigned); - } - - return this.sub(this.div(divisor).mul(divisor)); -}; - -/** - * Returns this Long modulo the specified. This is an alias of {@link Long#modulo}. - * @function - * @param {!Long|number|string} divisor Divisor - * @returns {!Long} Remainder - */ -LongPrototype.mod = LongPrototype.modulo; - -/** - * Returns this Long modulo the specified. This is an alias of {@link Long#modulo}. - * @function - * @param {!Long|number|string} divisor Divisor - * @returns {!Long} Remainder - */ -LongPrototype.rem = LongPrototype.modulo; - -/** - * Returns the bitwise NOT of this Long. - * @returns {!Long} - */ -LongPrototype.not = function not() { - return fromBits(~this.low, ~this.high, this.unsigned); -}; - -/** - * Returns the bitwise AND of this Long and the specified. - * @param {!Long|number|string} other Other Long - * @returns {!Long} - */ -LongPrototype.and = function and(other) { - if (!isLong(other)) - other = fromValue(other); - return fromBits(this.low & other.low, this.high & other.high, this.unsigned); -}; - -/** - * Returns the bitwise OR of this Long and the specified. - * @param {!Long|number|string} other Other Long - * @returns {!Long} - */ -LongPrototype.or = function or(other) { - if (!isLong(other)) - other = fromValue(other); - return fromBits(this.low | other.low, this.high | other.high, this.unsigned); -}; - -/** - * Returns the bitwise XOR of this Long and the given one. - * @param {!Long|number|string} other Other Long - * @returns {!Long} - */ -LongPrototype.xor = function xor(other) { - if (!isLong(other)) - other = fromValue(other); - return fromBits(this.low ^ other.low, this.high ^ other.high, this.unsigned); -}; - -/** - * Returns this Long with bits shifted to the left by the given amount. - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shiftLeft = function shiftLeft(numBits) { - if (isLong(numBits)) - numBits = numBits.toInt(); - if ((numBits &= 63) === 0) - return this; - else if (numBits < 32) - return fromBits(this.low << numBits, (this.high << numBits) | (this.low >>> (32 - numBits)), this.unsigned); - else - return fromBits(0, this.low << (numBits - 32), this.unsigned); -}; - -/** - * Returns this Long with bits shifted to the left by the given amount. This is an alias of {@link Long#shiftLeft}. - * @function - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shl = LongPrototype.shiftLeft; - -/** - * Returns this Long with bits arithmetically shifted to the right by the given amount. - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shiftRight = function shiftRight(numBits) { - if (isLong(numBits)) - numBits = numBits.toInt(); - if ((numBits &= 63) === 0) - return this; - else if (numBits < 32) - return fromBits((this.low >>> numBits) | (this.high << (32 - numBits)), this.high >> numBits, this.unsigned); - else - return fromBits(this.high >> (numBits - 32), this.high >= 0 ? 0 : -1, this.unsigned); -}; - -/** - * Returns this Long with bits arithmetically shifted to the right by the given amount. This is an alias of {@link Long#shiftRight}. - * @function - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shr = LongPrototype.shiftRight; - -/** - * Returns this Long with bits logically shifted to the right by the given amount. - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shiftRightUnsigned = function shiftRightUnsigned(numBits) { - if (isLong(numBits)) - numBits = numBits.toInt(); - numBits &= 63; - if (numBits === 0) - return this; - else { - var high = this.high; - if (numBits < 32) { - var low = this.low; - return fromBits((low >>> numBits) | (high << (32 - numBits)), high >>> numBits, this.unsigned); - } else if (numBits === 32) - return fromBits(high, 0, this.unsigned); - else - return fromBits(high >>> (numBits - 32), 0, this.unsigned); - } -}; - -/** - * Returns this Long with bits logically shifted to the right by the given amount. This is an alias of {@link Long#shiftRightUnsigned}. - * @function - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shru = LongPrototype.shiftRightUnsigned; - -/** - * Returns this Long with bits logically shifted to the right by the given amount. This is an alias of {@link Long#shiftRightUnsigned}. - * @function - * @param {number|!Long} numBits Number of bits - * @returns {!Long} Shifted Long - */ -LongPrototype.shr_u = LongPrototype.shiftRightUnsigned; - -/** - * Converts this Long to signed. - * @returns {!Long} Signed long - */ -LongPrototype.toSigned = function toSigned() { - if (!this.unsigned) - return this; - return fromBits(this.low, this.high, false); -}; - -/** - * Converts this Long to unsigned. - * @returns {!Long} Unsigned long - */ -LongPrototype.toUnsigned = function toUnsigned() { - if (this.unsigned) - return this; - return fromBits(this.low, this.high, true); -}; - -/** - * Converts this Long to its byte representation. - * @param {boolean=} le Whether little or big endian, defaults to big endian - * @returns {!Array.} Byte representation - */ -LongPrototype.toBytes = function toBytes(le) { - return le ? this.toBytesLE() : this.toBytesBE(); -}; - -/** - * Converts this Long to its little endian byte representation. - * @returns {!Array.} Little endian byte representation - */ -LongPrototype.toBytesLE = function toBytesLE() { - var hi = this.high, - lo = this.low; - return [ - lo & 0xff, - lo >>> 8 & 0xff, - lo >>> 16 & 0xff, - lo >>> 24 , - hi & 0xff, - hi >>> 8 & 0xff, - hi >>> 16 & 0xff, - hi >>> 24 - ]; -}; - -/** - * Converts this Long to its big endian byte representation. - * @returns {!Array.} Big endian byte representation - */ -LongPrototype.toBytesBE = function toBytesBE() { - var hi = this.high, - lo = this.low; - return [ - hi >>> 24 , - hi >>> 16 & 0xff, - hi >>> 8 & 0xff, - hi & 0xff, - lo >>> 24 , - lo >>> 16 & 0xff, - lo >>> 8 & 0xff, - lo & 0xff - ]; -}; - -/** - * Creates a Long from its byte representation. - * @param {!Array.} bytes Byte representation - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @param {boolean=} le Whether little or big endian, defaults to big endian - * @returns {Long} The corresponding Long value - */ -Long.fromBytes = function fromBytes(bytes, unsigned, le) { - return le ? Long.fromBytesLE(bytes, unsigned) : Long.fromBytesBE(bytes, unsigned); -}; - -/** - * Creates a Long from its little endian byte representation. - * @param {!Array.} bytes Little endian byte representation - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {Long} The corresponding Long value - */ -Long.fromBytesLE = function fromBytesLE(bytes, unsigned) { - return new Long( - bytes[0] | - bytes[1] << 8 | - bytes[2] << 16 | - bytes[3] << 24, - bytes[4] | - bytes[5] << 8 | - bytes[6] << 16 | - bytes[7] << 24, - unsigned - ); -}; - -/** - * Creates a Long from its big endian byte representation. - * @param {!Array.} bytes Big endian byte representation - * @param {boolean=} unsigned Whether unsigned or not, defaults to signed - * @returns {Long} The corresponding Long value - */ -Long.fromBytesBE = function fromBytesBE(bytes, unsigned) { - return new Long( - bytes[4] << 24 | - bytes[5] << 16 | - bytes[6] << 8 | - bytes[7], - bytes[0] << 24 | - bytes[1] << 16 | - bytes[2] << 8 | - bytes[3], - unsigned - ); -}; diff --git a/TelemetryDashboard/TelemetryDashboard.js b/TelemetryDashboard/TelemetryDashboard.js index 79f45f5b..ab9bb948 100644 --- a/TelemetryDashboard/TelemetryDashboard.js +++ b/TelemetryDashboard/TelemetryDashboard.js @@ -43,9 +43,15 @@ function setup_connect(button_svg, button_color) { const url_input = tip_div.querySelector(`input[id="target_url"]`) + const heartbeat_checkbox = tip_div.querySelector(`input[id="send_heartbeat"]`) + const passphrase_input = tip_div.querySelector(`input[id="signing_passphrase"]`) + const connect_button = tip_div.querySelector(`input[id="connection_button"]`) const disconnect_button = tip_div.querySelector(`input[id="disconnection_button"]`) + const system_id_input = tip_div.querySelector(`input[id="system_id"]`); + const component_id_input = tip_div.querySelector(`input[id="component_id"]`); + // Websocket object let ws = null let expecting_close = false @@ -60,6 +66,17 @@ function setup_connect(button_svg, button_color) { } set_inputs(false) + function apply_ids() { + // clamp to valid ranges; fall back to GCS-like defaults 255/190 + let sid = parseInt(system_id_input?.value || "255", 10); + let cid = parseInt(component_id_input?.value || "190", 10); + if (!Number.isFinite(sid) || sid < 1 || sid > 255) sid = 255; + if (!Number.isFinite(cid) || cid < 0 || cid > 255) cid = 190; + + MAVLink.srcSystem = sid; + MAVLink.srcComponent = cid; + } + // Connect to WebSocket server function connect(target, auto_connect) { // Make sure we are not connected to something else @@ -82,6 +99,9 @@ function setup_connect(button_svg, button_color) { ws.onopen = () => { button_color("green") + // setup system IDs + apply_ids(); + // Hide tip tip.hide() @@ -92,8 +112,47 @@ function setup_connect(button_svg, button_color) { url_input.value = target // Have been connected - been_connected = true - } + been_connected = true; + setup_passphrase = false; + + heartbeat_interval = setInterval(() => { + try { + if (!setup_passphrase) { + const passphrase = passphrase_input.value.trim(); + if (passphrase.length > 0) { + setup_passphrase = true; + const enc = new TextEncoder(); + const data = enc.encode(passphrase); + const hash = mavlink20.sha256(data); + MAVLink.signing.secret_key = new Uint8Array(hash); + MAVLink.signing.sign_outgoing = true; + } + } + + if (heartbeat_checkbox.checked) { + const msg = new mavlink20.messages.heartbeat( + 6, // MAV_TYPE_GCS + 8, // MAV_AUTOPILOT_INVALID + 0, // base_mode + 0, // custom_mode + 4 // MAV_STATE_ACTIVE + ); + const pkt = msg.pack(MAVLink); + ws.send(Uint8Array.from(pkt)); + //console.log("Sent HEARTBEAT", pkt); + } + } catch (e) { + console.error("Error sending HEARTBEAT:", e.message); + console.error(e.stack); + + if (heartbeat_interval !== null) { + clearInterval(heartbeat_interval) + heartbeat_interval = null + console.warn("Heartbeat disabled; will retry on reconnect.") + } + } + }, 1000); + } ws.onclose = () => { if ((auto_connect === true) && !been_connected) { @@ -115,10 +174,15 @@ function setup_connect(button_svg, button_color) { } ws.onmessage = (msg) => { - // Feed data to MAVLink parser and forward messages - for (const char of new Uint8Array(msg.data)) { - const m = MAVLink.parseChar(char) - if ((m != null) && (m._id != -1)) { + // Feed data to MAVLink parser and forward messages + MAVLink.pushBuffer(new Uint8Array(msg.data)); + while (true) { + const m = MAVLink.parseChar(null); + if (m === null) { + break; + } + if (m._id != -1) { + //console.log(m); // Got message with known ID // Sent to each Widget for (const widget of grid.getGridItems()) { @@ -130,7 +194,6 @@ function setup_connect(button_svg, button_color) { } } } - } // Disconnect from WebSocket server diff --git a/TelemetryDashboard/Widgets/SandBox.html b/TelemetryDashboard/Widgets/SandBox.html index e7cb8671..5303c29e 100644 --- a/TelemetryDashboard/Widgets/SandBox.html +++ b/TelemetryDashboard/Widgets/SandBox.html @@ -4,7 +4,7 @@ Sandbox - + diff --git a/TelemetryDashboard/cli_test.js b/TelemetryDashboard/cli_test.js new file mode 100644 index 00000000..e0231bfb --- /dev/null +++ b/TelemetryDashboard/cli_test.js @@ -0,0 +1,113 @@ +//!/usr/bin/env node + +const WebSocket = require('ws'); + +global.jspack = new (require('../modules/MAVLink/local_modules/jspack/jspack.js')).default; + +const mavlib = require('../modules/MAVLink/mavlink.js'); // Use local MAVLink definition + +if (process.argv.length < 3) { + console.error("Usage: node ws_mavlink.js "); + process.exit(1); +} + +//process.env.NODE_TLS_REJECT_UNAUTHORIZED = "0"; + +const url = process.argv[2]; +const ws = new WebSocket(url); +ws.binaryType = "arraybuffer"; + +// Create a MAVLink v2 parser +parser = new MAVLink20Processor() + +let signing_passphrase = null; +if (process.argv.length > 3) { + signing_passphrase = process.argv[3]; +} + +const crypto = require('crypto'); + +function passphrase_to_key(passphrase) { + return crypto.createHash('sha256').update(Buffer.from(passphrase, 'ascii')).digest(); +} + +if (signing_passphrase) { + parser.signing.secret_key = passphrase_to_key(signing_passphrase); + parser.signing.sign_outgoing = true; +} + +let heartbeat_interval; + +ws.on('open', () => { + console.log(`Connected to ${url}`); + + heartbeat_interval = setInterval(() => { + try { + const msg = new mavlib.mavlink20.messages.heartbeat( + 6, // MAV_TYPE_GCS + 8, // MAV_AUTOPILOT_INVALID + 0, // base_mode + 0, // custom_mode + 4 // MAV_STATE_ACTIVE + ); + const pkt = msg.pack(parser); + ws.send(pkt); + console.log("Sent HEARTBEAT"); + } catch (e) { + console.error("Error sending HEARTBEAT:", e.message); + console.error(e.stack); + } + }, 1000); +}); + +function mav_pretty(msg) { + // console.log(JSON.stringify(msg, null, 2)); + + if (!msg || !msg._name || !msg.fieldnames) { + return ""; + } + + const name = msg._name; + const fields = msg.fieldnames + .map(fn => { + let val = msg[fn]; + if (typeof val === "string") { + val = `"${val}"`; + } else if (Array.isArray(val)) { + val = "[" + val.join(", ") + "]"; + } + return `${fn}=${val}`; + }) + .join(", "); + + return `${name} { ${fields} }`; +} + + +ws.on('message', (data) => { + const buf = (data instanceof ArrayBuffer) ? Buffer.from(data) : + (Buffer.isBuffer(data) ? data : Buffer.from(data.buffer || data)); + + console.log(`Received ${buf.length} bytes: [${buf.slice(0, 10).toString('hex')}...]`); + + for (const b of buf) { + try { + const msg = parser.parseChar(b); + if (msg) { + console.log(`MAVLink message ID: ${msg._id}`); + console.log(mav_pretty(msg)); + } + } catch (e) { + console.warn(`Parser error on byte 0x${byte.toString(16)}: ${e.message}`); + } + } +}); + +ws.on('close', () => { + console.log("WebSocket closed"); + clearInterval(heartbeat_interval); +}); + +ws.on('error', (err) => { + console.error("WebSocket error:", err.message); +}); diff --git a/TelemetryDashboard/index.html b/TelemetryDashboard/index.html index 1c4fe8ee..ce4c386f 100644 --- a/TelemetryDashboard/index.html +++ b/TelemetryDashboard/index.html @@ -23,7 +23,7 @@ - + @@ -176,6 +176,21 @@
Dashboard settings


+
+ + +
+
+ + + + +
+
+
+ + +

@@ -200,20 +215,21 @@
Dashboard settings
}) // Load grid - let grid - let grid_changed = false - load_default_grid() + let grid; + let grid_changed = false; // MAVLink parsing - MAVLink = new MAVLink20Processor() + let MAVLink = new MAVLink20Processor(); + + load_default_grid(); // Setup editor for use later - init_editor() + init_editor(); // Setup widget pallet - init_pallet() + init_pallet(); // Bind unload event to allow prompt for save - window.addEventListener('beforeunload', handle_unload) + window.addEventListener('beforeunload', handle_unload); diff --git a/images/SimpleGCS_Icon.png b/images/SimpleGCS_Icon.png new file mode 100644 index 00000000..96da448e Binary files /dev/null and b/images/SimpleGCS_Icon.png differ diff --git a/modules/MAVLink/local_modules/README.md b/modules/MAVLink/local_modules/README.md new file mode 100644 index 00000000..ad5a1382 --- /dev/null +++ b/modules/MAVLink/local_modules/README.md @@ -0,0 +1,6 @@ +This folder is a locally modified copy of some Node/npm packages 'jspack' and 'long'. we have copied them here and tweaked them to be compatible with our needs, please see their respective README.md file for their original info, which we have not changed. + +This README.md serves to make you aware that these two packages as stored here in the 'jspack' and 'long' folders ARE MODIFIED from the originals. +By placing this statement here, and putting a notice in long.js as well, we feel are in compliance with the LICENSE file of 'long' , which requires us to tell you they are modified. + +We have included their original license files, in compliance with them, as both license/s permit distribution of derived works in source and/or binary form. diff --git a/modules/MAVLink/local_modules/jspack/.npmignore b/modules/MAVLink/local_modules/jspack/.npmignore new file mode 100644 index 00000000..be1ea81a --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/.npmignore @@ -0,0 +1,19 @@ +lib-cov +*.seed +*.log +*.csv +*.dat +*.out +*.pid +*.gz +*.pyc + +pids +logs +results + +npm-debug.log +node_modules + +**~ +**.swp diff --git a/modules/MAVLink/local_modules/jspack/LICENSE b/modules/MAVLink/local_modules/jspack/LICENSE new file mode 100644 index 00000000..d646dd72 --- /dev/null +++ b/modules/MAVLink/local_modules/jspack/LICENSE @@ -0,0 +1,26 @@ +Copyright (c) 2008, Fair Oaks Labs, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or other + materials provided with the distribution. + + * Neither the name of Fair Oaks Labs, Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without specific + prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL +THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF +THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/TelemetryDashboard/MAVLink/local_modules/jspack/README.md b/modules/MAVLink/local_modules/jspack/README.md similarity index 100% rename from TelemetryDashboard/MAVLink/local_modules/jspack/README.md rename to modules/MAVLink/local_modules/jspack/README.md diff --git a/TelemetryDashboard/MAVLink/local_modules/jspack/jspack.js b/modules/MAVLink/local_modules/jspack/jspack.js similarity index 100% rename from TelemetryDashboard/MAVLink/local_modules/jspack/jspack.js rename to modules/MAVLink/local_modules/jspack/jspack.js diff --git a/TelemetryDashboard/MAVLink/local_modules/jspack/package.json b/modules/MAVLink/local_modules/jspack/package.json similarity index 100% rename from TelemetryDashboard/MAVLink/local_modules/jspack/package.json rename to modules/MAVLink/local_modules/jspack/package.json diff --git a/TelemetryDashboard/MAVLink/local_modules/jspack/test/int64.js b/modules/MAVLink/local_modules/jspack/test/int64.js similarity index 100% rename from TelemetryDashboard/MAVLink/local_modules/jspack/test/int64.js rename to modules/MAVLink/local_modules/jspack/test/int64.js diff --git a/modules/MAVLink/mavftp.js b/modules/MAVLink/mavftp.js new file mode 100644 index 00000000..7fa6fd8e --- /dev/null +++ b/modules/MAVLink/mavftp.js @@ -0,0 +1,406 @@ +// MAVLink FTP implementation for fetching fence data +// Based on MAVProxy's mavproxy_ftp.py + +class MAVFTP { + constructor(mavlink, ws) { + this.MAVLink = mavlink; + this.ws = ws; + + // Operation codes + this.OP = { + None: 0, + TerminateSession: 1, + ResetSessions: 2, + ListDirectory: 3, + OpenFileRO: 4, + ReadFile: 5, + CreateFile: 6, + WriteFile: 7, + RemoveFile: 8, + CreateDirectory: 9, + RemoveDirectory: 10, + OpenFileWO: 11, + TruncateFile: 12, + Rename: 13, + CalcFileCRC32: 14, + BurstReadFile: 15, + Ack: 128, + Nack: 129 + }; + + // Error codes + this.ERR = { + None: 0, + Fail: 1, + FailErrno: 2, + InvalidDataSize: 3, + InvalidSession: 4, + NoSessionsAvailable: 5, + EndOfFile: 6, + UnknownCommand: 7, + FileExists: 8, + FileProtected: 9, + FileNotFound: 10 + }; + + // FTP state + this.seq = 0; + this.session = 0; + this.targetSystem = 1; + this.targetComponent = 1; + + // File transfer state + this.currentFile = null; + this.fileBuffer = null; + this.readGaps = []; + this.reachedEOF = false; + this.burstSize = 80; + this.lastOp = null; + this.opStartTime = null; + this.callback = null; + + this.HDR_LEN = 12; + this.MAX_PAYLOAD = 239; + } + + // Pack FTP operation into bytes + packOp(seq, session, opcode, size, req_opcode, burst_complete, offset, payload) { + const buffer = new ArrayBuffer(this.HDR_LEN + this.MAX_PAYLOAD); + const view = new DataView(buffer); + + // Header: seq(u16), session(u8), opcode(u8), size(u8), req_opcode(u8), burst_complete(u8), pad(u8), offset(u32) + view.setUint16(0, seq, true); + view.setUint8(2, session); + view.setUint8(3, opcode); + view.setUint8(4, size); + view.setUint8(5, req_opcode); + view.setUint8(6, burst_complete); + view.setUint8(7, 0); // pad + view.setUint32(8, offset, true); + + // Payload + if (payload) { + const payloadBytes = new Uint8Array(buffer, 12); + for (let i = 0; i < Math.min(payload.length, this.MAX_PAYLOAD); i++) { + payloadBytes[i] = payload[i]; + } + } + + return new Uint8Array(buffer); + } + + // Parse FTP operation from FILE_TRANSFER_PROTOCOL message + parseOp(payload) { + // Convert payload to Uint8Array if it's not already + // Handle case where payload is a plain array or array-like object + if (!(payload instanceof Uint8Array)) { + payload = new Uint8Array([...payload].map((c) => c.charCodeAt(0))); + } + + // Ensure we have at least the header + if (payload.length < this.HDR_LEN) { + console.error(`FTP: Payload too short (${payload.length} bytes)`); + return null; + } + + const view = new DataView(payload.buffer, payload.byteOffset, payload.byteLength); + + const op = { + seq: view.getUint16(0, true), + session: view.getUint8(2), + opcode: view.getUint8(3), + size: view.getUint8(4), + req_opcode: view.getUint8(5), + burst_complete: view.getUint8(6), + offset: view.getUint32(8, true), + payload: null + }; + + // Extract payload if present and size > 0 + if (op.size > 0 && payload.length > this.HDR_LEN) { + const payloadStart = this.HDR_LEN; + const payloadLength = Math.min(op.size, payload.length - this.HDR_LEN); + op.payload = new Uint8Array(payload.buffer, payload.byteOffset + payloadStart, payloadLength); + } + + return op; + } + + // Send FTP operation + sendOp(opcode, size, req_opcode, burst_complete, offset, payload) { + const packedOp = this.packOp(this.seq, this.session, opcode, size, req_opcode, burst_complete, offset, payload); + + // Send via FILE_TRANSFER_PROTOCOL message + const msg = new mavlink20.messages.file_transfer_protocol( + 0, // network (0 for Mavlink 2.0) + this.targetSystem, + this.targetComponent, + Array.from(packedOp) + ); + + const pkt = msg.pack(this.MAVLink); + this.ws.send(Uint8Array.from(pkt)); + + this.lastOp = { opcode, size, req_opcode, burst_complete, offset, payload }; + + console.log(`FTP: Sent op ${opcode} (${this.getOpName(opcode)}) seq=${this.seq} session=${this.session} offset=${offset}`); + this.seq = (this.seq + 1) % 256; + } + + // Helper to get operation name for debugging + getOpName(opcode) { + for (let [name, code] of Object.entries(this.OP)) { + if (code === opcode) return name; + } + return `Unknown(${opcode})`; + } + + // Terminate current session + terminateSession() { + this.sendOp(this.OP.TerminateSession, 0, 0, 0, 0, null); + this.session = (this.session + 1) % 256; + this.currentFile = null; + this.fileBuffer = null; + this.readGaps = []; + this.reachedEOF = false; + console.log("FTP: Session terminated"); + } + + // Get file from vehicle + getFile(filename, callback) { + console.log(`FTP: Getting file ${filename}`); + + this.terminateSession(); + this.callback = callback; + this.currentFile = filename; + this.fileBuffer = new Uint8Array(0); + this.readGaps = []; + this.reachedEOF = false; + this.opStartTime = Date.now(); + + // Encode filename + const encoder = new TextEncoder(); + const filenameBytes = encoder.encode(filename); + + // Send OpenFileRO + this.sendOp(this.OP.OpenFileRO, filenameBytes.length, 0, 0, 0, filenameBytes); + } + + // Handle incoming FILE_TRANSFER_PROTOCOL message + handleMessage(m) { + if (m._name !== "FILE_TRANSFER_PROTOCOL") return; + if (m.target_system !== this.MAVLink.srcSystem || m.target_component !== this.MAVLink.srcComponent) return; + + // m.payload is already a Uint8Array, pass it directly + const op = this.parseOp(m.payload); + if (!op) { + console.error("FTP: Failed to parse operation"); + return; + } + + console.log(`FTP: Received ${this.getOpName(op.opcode)} for req=${this.getOpName(op.req_opcode)} size=${op.size} offset=${op.offset} seq=${op.seq}`); + + // Handle different response types + if (op.req_opcode === this.OP.OpenFileRO) { + this.handleOpenResponse(op); + } else if (op.req_opcode === this.OP.BurstReadFile) { + this.handleBurstReadResponse(op); + } else if (op.req_opcode === this.OP.ReadFile) { + this.handleReadResponse(op); + } else if (op.req_opcode === this.OP.TerminateSession) { + console.log("FTP: Session terminated ACK"); + } else { + console.log(`FTP: Unhandled response for ${this.getOpName(op.req_opcode)}`); + } + } + + // Handle OpenFileRO response + handleOpenResponse(op) { + if (op.opcode === this.OP.Ack) { + console.log("FTP: File opened, starting burst read"); + // Start burst read from offset 0 + this.sendOp(this.OP.BurstReadFile, this.burstSize, 0, 0, 0, null); + } else if (op.opcode === this.OP.Nack) { + console.error("FTP: Failed to open file - NACK received"); + if (this.callback) this.callback(null); + this.terminateSession(); + } else { + console.error(`FTP: Unexpected response to OpenFileRO: opcode ${op.opcode}`); + } + } + + // Handle BurstReadFile response + handleBurstReadResponse(op) { + if (op.opcode === this.OP.Ack && op.payload) { + if (!this.fileBuffer) { + return; + } + // Expand buffer if needed + const newSize = op.offset + op.size; + if (newSize > this.fileBuffer.length) { + const newBuffer = new Uint8Array(newSize); + newBuffer.set(this.fileBuffer); + this.fileBuffer = newBuffer; + } + + // Write data at offset + this.fileBuffer.set(op.payload, op.offset); + + console.log(`FTP: Read ${op.size} bytes at offset ${op.offset}`); + + // Check if we need to continue + if (op.burst_complete) { + if (op.size > 0 && op.size < this.burstSize) { + // EOF reached + this.reachedEOF = true; + this.finishTransfer(); + } else { + // Continue reading + const nextOffset = op.offset + op.size; + this.sendOp(this.OP.BurstReadFile, this.burstSize, 0, 0, nextOffset, null); + } + } + } else if (op.opcode === this.OP.Nack) { + const errorCode = op.payload ? op.payload[0] : 0; + if (errorCode === this.ERR.EndOfFile || errorCode === 0) { + console.log("FTP: EOF reached"); + this.reachedEOF = true; + this.finishTransfer(); + } else { + console.error(`FTP: Read failed with error ${errorCode}`); + if (this.callback) this.callback(null); + this.terminateSession(); + } + } + } + + // Handle ReadFile response (for gap filling) + handleReadResponse(op) { + if (op.opcode === this.OP.Ack && op.payload) { + // Fill gap + this.fileBuffer.set(op.payload, op.offset); + + // Remove from gaps list + this.readGaps = this.readGaps.filter(g => g.offset !== op.offset); + + console.log(`FTP: Filled gap at ${op.offset}, ${this.readGaps.length} gaps remaining`); + + if (this.readGaps.length === 0 && this.reachedEOF) { + this.finishTransfer(); + } + } + } + + // Finish file transfer + finishTransfer() { + if (!this.fileBuffer) { + return; + } + const dt = (Date.now() - this.opStartTime) / 1000; + const size = this.fileBuffer.length; + const rate = (size / dt) / 1024; + + console.log(`FTP: Transfer complete - ${size} bytes in ${dt.toFixed(2)}s (${rate.toFixed(1)} KB/s)`); + + if (this.callback) { + this.callback(this.fileBuffer); + } + + this.terminateSession(); + } +} + +// Fence data parser +class FenceParser { + constructor() { + } + + parseMissionItems(data) { + var header = data.buffer.slice(0,10); + var hdr = jspack.Unpack(" { - jspack = new mod.default() -}).catch((e) => { - // Discard annoying error. - // This fails in the sandbox because of CORS - // Only really want the message definitions in that case, not the parser, so its OK. -}) +// Detect environment +const isNode = typeof process !== 'undefined' && + process.versions && + process.versions.node; + +// Handle jspack dependency +let jspack; +if (isNode) { + jspack = (global && global.jspack) || require("jspack").jspack; +} else { + import("./local_modules/jspack/jspack.js").then((mod) => { + jspack = new mod.default() + }).catch((e) => { + }); +} + +// Handle Node.js specific modules +let events, util; +if (isNode) { + events = require("events"); + util = require("util"); +} else { + // Browser polyfills for Node.js modules + util = { + inherits: function(constructor, superConstructor) { + constructor.prototype = Object.create(superConstructor.prototype); + constructor.prototype.constructor = constructor; + } + }; + + // Simple EventEmitter polyfill for browsers + events = { + EventEmitter: function() { + this._events = {}; + + this.on = function(event, listener) { + if (!this._events[event]) { + this._events[event] = []; + } + this._events[event].push(listener); + }; + + this.emit = function(event, ...args) { + if (this._events[event]) { + this._events[event].forEach(listener => { + try { + listener.apply(this, args); + } catch (e) { + console.error('Error in event listener:', e); + } + }); + } + }; + + this.removeListener = function(event, listener) { + if (this._events[event]) { + const index = this._events[event].indexOf(listener); + if (index > -1) { + this._events[event].splice(index, 1); + } + } + }; + } + }; +} + mavlink20 = function(){}; @@ -55,7 +113,7 @@ mavlink20.MAVLINK_IFLAG_SIGNED = 0x01 mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN = 13 // Mavlink headers incorporate sequence, source system (platform) and source component. -mavlink20.header = function(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags=0, compat_flags=0,) { +mavlink20.header = function(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags=0, compat_flags=0) { this.mlen = ( typeof mlen === 'undefined' ) ? 0 : mlen; this.seq = ( typeof seq === 'undefined' ) ? 0 : seq; @@ -90,36 +148,140 @@ mavlink20.message.prototype.set = function(args,verbose) { }, this); }; -// trying to be the same-ish as the python function of the same name -mavlink20.message.prototype.sign_packet = function( mav) { - var crypto= require('crypto'); - var h = crypto.createHash('sha256'); +/* + sha256 implementation + embedded to avoid async issues in web browsers with crypto library + with thanks to https://geraintluff.github.io/sha256/ +*/ +mavlink20.sha256 = function(inputBytes) { + const K = new Uint32Array([ + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, + 0x59f111f1, 0x923f82a4, 0xab1c5ed5, 0xd807aa98, 0x12835b01, + 0x243185be, 0x550c7dc3, 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, + 0xc19bf174, 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, 0x983e5152, + 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, + 0x06ca6351, 0x14292967, 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, + 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, + 0xd6990624, 0xf40e3585, 0x106aa070, 0x19a4c116, 0x1e376c08, + 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, + 0x682e6ff3, 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 + ]); + + function ROTR(n, x) { return (x >>> n) | (x << (32 - n)); } + + function Σ0(x) { return ROTR(2, x) ^ ROTR(13, x) ^ ROTR(22, x); } + function Σ1(x) { return ROTR(6, x) ^ ROTR(11, x) ^ ROTR(25, x); } + function σ0(x) { return ROTR(7, x) ^ ROTR(18, x) ^ (x >>> 3); } + function σ1(x) { return ROTR(17, x) ^ ROTR(19, x) ^ (x >>> 10); } + + function Ch(x, y, z) { return (x & y) ^ (~x & z); } + function Maj(x, y, z) { return (x & y) ^ (x & z) ^ (y & z); } + + const H = new Uint32Array([ + 0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, + 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19 + ]); + + const l = inputBytes.length; + const bitLen = l * 8; + + const withPadding = new Uint8Array(((l + 9 + 63) >> 6) << 6); // pad to multiple of 64 bytes + withPadding.set(inputBytes); + withPadding[l] = 0x80; + withPadding.set([ + 0, 0, 0, 0, + (bitLen >>> 24) & 0xff, + (bitLen >>> 16) & 0xff, + (bitLen >>> 8) & 0xff, + bitLen & 0xff + ], withPadding.length - 8); + + const w = new Uint32Array(64); + for (let i = 0; i < withPadding.length; i += 64) { + for (let j = 0; j < 16; j++) { + w[j] = ( + (withPadding[i + 4 * j] << 24) | + (withPadding[i + 4 * j + 1] << 16) | + (withPadding[i + 4 * j + 2] << 8) | + (withPadding[i + 4 * j + 3]) + ) >>> 0; + } + for (let j = 16; j < 64; j++) { + w[j] = (σ1(w[j - 2]) + w[j - 7] + σ0(w[j - 15]) + w[j - 16]) >>> 0; + } - //mav.signing.timestamp is a 48bit number, or 6 bytes. + let [a, b, c, d, e, f, g, h] = H; + + for (let j = 0; j < 64; j++) { + const T1 = (h + Σ1(e) + Ch(e, f, g) + K[j] + w[j]) >>> 0; + const T2 = (Σ0(a) + Maj(a, b, c)) >>> 0; + h = g; + g = f; + f = e; + e = (d + T1) >>> 0; + d = c; + c = b; + b = a; + a = (T1 + T2) >>> 0; + } + + H[0] = (H[0] + a) >>> 0; + H[1] = (H[1] + b) >>> 0; + H[2] = (H[2] + c) >>> 0; + H[3] = (H[3] + d) >>> 0; + H[4] = (H[4] + e) >>> 0; + H[5] = (H[5] + f) >>> 0; + H[6] = (H[6] + g) >>> 0; + H[7] = (H[7] + h) >>> 0; + } + + const output = new Uint8Array(32); + for (let i = 0; i < 8; i++) { + output[i * 4 + 0] = (H[i] >>> 24) & 0xff; + output[i * 4 + 1] = (H[i] >>> 16) & 0xff; + output[i * 4 + 2] = (H[i] >>> 8) & 0xff; + output[i * 4 + 3] = H[i] & 0xff; + } + + return output; +} - // due to js not being able to shift numbers more than 32, we'll use this instead.. - // js stores all its numbers as a 64bit float with 53 bits of mantissa, so have room for 48 ok. - // positive shifts left, negative shifts right - function shift(number, shift) { - return number * Math.pow(2, shift); - } +// create a message signature +mavlink20.create_signature = function(key, msgbuf) { + const input = new Uint8Array(32 + msgbuf.length); + input.set(key, 0); + input.set(msgbuf, 32); - var thigh = shift(mav.signing.timestamp,-32) // 2 bytes from the top, shifted right by 32 bits - var tlow = (mav.signing.timestamp & 0xfffffff ) // 4 bytes from the bottom + const hash = mavlink20.sha256(input); + const sig = hash.slice(0, 6); + + return sig; +} + +// sign outgoing packet +mavlink20.message.prototype.sign_packet = function( mav) { + function packUint48LE(value) { + const bytes = [] + for (let i = 0; i < 6; i++) { + bytes.push(Number((value >> BigInt(8 * i)) & 0xFFn)); + } + return bytes; + } + + var tsbuf = packUint48LE(BigInt(mav.signing.timestamp)); - // I means unsigned 4bytes, H means unsigned 2 bytes // first add the linkid(1 byte) and timestamp(6 bytes) that start the signature - this._msgbuf = this._msgbuf.concat(jspack.Pack(' 0 indicates the interval at which it is sent. (int32_t) @@ -13702,7 +13974,7 @@ for the missing image. relative_alt : Altitude above ground (int32_t) q : Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) (float) image_index : Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) (int32_t) - capture_result : Boolean indicating success (1) or failure (0) while capturing this image. (int8_t) + capture_result : Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. (int8_t) file_url : URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. (char) */ @@ -13813,7 +14085,7 @@ A message containing logged data (see also MAV_CMD_LOGGING_START) target_component : component ID of the target (uint8_t) sequence : sequence number (can wrap) (uint16_t) length : data length (uint8_t) - first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). (uint8_t) + first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). (uint8_t) data : logged data (uint8_t) */ @@ -13852,7 +14124,7 @@ sent back target_component : component ID of the target (uint8_t) sequence : sequence number (can wrap) (uint16_t) length : data length (uint8_t) - first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). (uint8_t) + first_message_offset : offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). (uint8_t) data : logged data (uint8_t) */ @@ -14138,6 +14410,54 @@ mavlink20.messages.camera_tracking_geo_status.prototype.pack = function(mav) { } +/* +Camera absolute thermal range. This can be streamed when the +associated `VIDEO_STREAM_STATUS.flag` bit +`VIDEO_STREAM_STATUS_FLAGS_THERMAL_RANGE_ENABLED` is set, but a GCS +may choose to only request it for the current active stream. Use +MAV_CMD_SET_MESSAGE_INTERVAL to define message interval (param3 +indicates the stream id of the current camera, or 0 for all streams, +param4 indicates the target camera_device_id for autopilot-attached +cameras or 0 for MAVLink cameras). + + time_boot_ms : Timestamp (time since system boot). (uint32_t) + stream_id : Video Stream ID (1 for first, 2 for second, etc.) (uint8_t) + camera_device_id : Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). (uint8_t) + max : Temperature max. (float) + max_point_x : Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. (float) + max_point_y : Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. (float) + min : Temperature min. (float) + min_point_x : Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. (float) + min_point_y : Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. (float) + +*/ + mavlink20.messages.camera_thermal_range = function( ...moreargs ) { + [ this.time_boot_ms , this.stream_id , this.camera_device_id , this.max , this.max_point_x , this.max_point_y , this.min , this.min_point_x , this.min_point_y ] = moreargs; + + this._format = '= 1 && this.buf[0] != this.protocol_marker ) { + if( this.buf.length >= 1 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V2 && + this.buf[0] != mavlink20.PROTOCOL_MARKER_V1) { - // Strip the offending initial byte and throw an error. + // Strip the offending initial bytes and throw an error. var badPrefix = this.buf[0]; - this.bufInError = this.buf.slice(0,1); - this.buf = this.buf.slice(1); + var idx1 = this.buf.indexOf(mavlink20.PROTOCOL_MARKER_V1); + var idx2 = this.buf.indexOf(mavlink20.PROTOCOL_MARKER_V2); + if (idx1 == -1) { + idx1 = idx2; + } + if (idx1 == -1 && idx2 == -1) { + this.bufInError = this.buf; + this.buf = new Uint8Array(); + } else { + this.bufInError = this.buf.slice(0,idx1); + this.buf = this.buf.slice(idx1); + } this.expected_length = mavlink20.HEADER_LEN; //initially we 'expect' at least the length of the header, later parseLength corrects for this. throw new Error("Bad prefix ("+badPrefix+")"); } @@ -18403,31 +19112,30 @@ MAVLink20Processor.prototype.parsePrefix = function() { // us know if we have signing enabled, which affects the real-world length by the signature-block length of 13 bytes. // once successful, 'this.expected_length' is correctly set for the whole packet. MAVLink20Processor.prototype.parseLength = function() { - - if( this.buf.length >= 3 ) { - var unpacked = jspack.Unpack('BBB', this.buf.slice(0, 3)); - var magic = unpacked[0]; // stx ie fd or fe etc - this.expected_length = unpacked[1] + mavlink20.HEADER_LEN + 2 // length of message + header + CRC (ie non-signed length) - this.incompat_flags = unpacked[2]; - // mavlink2 only.. in mavlink1, incompat_flags var above is actually the 'seq', but for this test its ok. - if ((magic == mavlink20.PROTOCOL_MARKER_V2 ) && ( this.incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED )){ - this.expected_length += mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; - } + + if( this.buf.length >= 3 ) { + var unpacked = jspack.Unpack('BBB', this.buf.slice(0, 3)); + var magic = unpacked[0]; // stx ie fd or fe etc + this.expected_length = unpacked[1] + mavlink20.HEADER_LEN + 2 // length of message + header + CRC (ie non-signed length) + this.incompat_flags = unpacked[2]; + // mavlink2 only.. in mavlink1, incompat_flags var above is actually the 'seq', but for this test its ok. + if ((magic == mavlink20.PROTOCOL_MARKER_V2 ) && ( this.incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED )){ + this.expected_length += mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; + } } } -// input some data bytes, possibly returning a new message - python equiv function is called parse_char / __parse_char_legacy +// input some data bytes, possibly returning a new message - python equiv function is called parse_char / __parse_char_legacy +// c can be null to process any remaining data in the input buffer from a previous call MAVLink20Processor.prototype.parseChar = function(c) { - if (c == null) { - return - } - var m = null; try { - this.pushBuffer(c); + if (c != null) { + this.pushBuffer(c); + } this.parsePrefix(); this.parseLength(); m = this.parsePayload(); @@ -18441,10 +19149,10 @@ MAVLink20Processor.prototype.parseChar = function(c) { } // emit a packet-specific message as well as a generic message, user/s can choose to use either or both of these. - //if(null != m) { - // this.emit(m._name, m); - // this.emit('message', m); - //} + if (isNode && null != m) { + this.emit(m._name, m); + this.emit('message', m); + } return m; @@ -18488,7 +19196,7 @@ MAVLink20Processor.prototype.parsePayload = function() { // input some data bytes, possibly returning an array of new messages MAVLink20Processor.prototype.parseBuffer = function(s) { - + // Get a message, if one is available in the stream. var m = this.parseChar(s); @@ -18496,12 +19204,13 @@ MAVLink20Processor.prototype.parseBuffer = function(s) { if ( null === m ) { return null; } - + // While more valid messages can be read from the existing buffer, add // them to the array of new messages and return them. - var ret = [m]; + var ret = []; + ret.push(m); while(true) { - m = this.parseChar(); + m = this.parseChar(null); if ( null === m ) { // No more messages left. return ret; @@ -18513,94 +19222,84 @@ MAVLink20Processor.prototype.parseBuffer = function(s) { //check signature on incoming message , many of the comments in this file come from the python impl MAVLink20Processor.prototype.check_signature = function(msgbuf, srcSystem, srcComponent) { - - //timestamp_buf = msgbuf[-12:-6] - var timestamp_buf= msgbuf.slice(-12,-6); - - //link_id = msgbuf[-13] - var link_id= new Buffer.from(msgbuf.slice(-13,-12)); // just a single byte really, but returned as a buffer - link_id = link_id[0]; // get the first byte. + var timestamp_buf = msgbuf.slice(-12,-6); - //self.mav_sign_unpacker = jspack.Unpack('= 0; i--) { + value = (value << 8n) | BigInt(bytes[i]); + } + return value; + } + var timestamp = Number(unpackUint48LE(timestamp_buf)); + + // see if the timestamp is acceptable + + // we'll use a STRING containing these three things in it as a unique key eg: '0,1,1' + stream_key = new Array(link_id,srcSystem,srcComponent).toString(); + + if (stream_key in this.signing.stream_timestamps){ + if (timestamp <= this.signing.stream_timestamps[stream_key]){ + //# reject old timestamp + //console.log('old timestamp') + return false + } + }else{ + //# a new stream has appeared. Accept the timestamp if it is at most + //# one minute behind our current timestamp + if (timestamp + 6000*1000 < this.signing.timestamp){ + //console.log('bad new stream ', timestamp/(100.0*1000*60*60*24*365), this.signing.timestamp/(100.0*1000*60*60*24*365)) + return false + } + this.signing.stream_timestamps[stream_key] = timestamp; + //console.log('new stream',this.signing.stream_timestamps) + } + + // just the last 6 of 13 available are the actual sig . ie excluding the linkid(1) and timestamp(6) + var sigpart = msgbuf.slice(-6); + sigpart = Uint8Array.from(sigpart); + // not sig part 0- end-minus-6 + var notsigpart = msgbuf.slice(0,-6); + notsigpart = Uint8Array.from(notsigpart); + + var sig1 = mavlink20.create_signature(this.signing.secret_key, notsigpart); + + // Browser-compatible buffer comparison + var signaturesMatch; + if (isNode) { + signaturesMatch = Buffer.from(sig1).equals(Buffer.from(sigpart)); + } else { + // Compare arrays element by element in browser + signaturesMatch = sig1.length === sigpart.length && + sig1.every((val, index) => val === sigpart[index]); + } + if (!signaturesMatch) { + return false; + } + //# the timestamp we next send with is the max of the received timestamp and + //# our current timestamp + this.signing.timestamp = Math.max(this.signing.timestamp, timestamp+1); + return true +} /* decode a buffer as a MAVLink message */ MAVLink20Processor.prototype.decode = function(msgbuf) { - var magic, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent, unpacked, msgId, signature_len; + var magic, incompat_flags, compat_flags, mlen, seq, srcSystem, srcComponent, unpacked, msgId, signature_len, header_len; // decode the header try { - unpacked = jspack.Unpack('BBBBBBBHB', msgbuf.slice(0, 10)); // the H in here causes msgIDlow to takeup 2 bytes, the rest 1 + if (msgbuf[0] == 253) { + var unpacked = jspack.Unpack('BBBBBBBHB', msgbuf.slice(0, 10)); // the H in here causes msgIDlow to takeup 2 bytes, the rest 1 magic = unpacked[0]; mlen = unpacked[1]; incompat_flags = unpacked[2]; @@ -18610,64 +19309,51 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { srcComponent = unpacked[6]; var msgIDlow = ((unpacked[7] & 0xFF) << 8) | ((unpacked[7] >> 8) & 0xFF); // first-two msgid bytes var msgIDhigh = unpacked[8]; // the 3rd msgid byte - msgId = msgIDlow | (msgIDhigh<<16); // combined result. 0 - 16777215 24bit number + msgId = msgIDlow | (msgIDhigh<<16); // combined result. 0 - 16777215 24bit number + header_len = 10; +} else { + var unpacked = jspack.Unpack('BBBBBB', msgbuf.slice(0, 6)); + magic = unpacked[0]; + mlen = unpacked[1]; + seq = unpacked[2]; + srcSystem = unpacked[3]; + srcComponent = unpacked[4]; + msgID = unpacked[5]; + incompat_flags = 0; + compat_flags = 0; + header_len = 6; +} } catch(e) { throw new Error('Unable to unpack MAVLink header: ' + e.message); } - // TODO allow full parsing of 1.0 inside the 2.0 parser, this is just a start - if (magic == mavlink20.PROTOCOL_MARKER_V1){ - //headerlen = 6; - - // these two are in the same place in both v1 and v2 so no change needed: - //magic = magic; - //mlen = mlen; - - // grab mavlink-v1 header position info from v2 unpacked position - seq1 = incompat_flags; - srcSystem1 = compat_flags; - srcComponent1 = seq; - msgId1 = srcSystem; - // override the v1 vs v2 offsets so we get the correct data either way... - seq = seq1; - srcSystem = srcSystem1; - srcComponent = srcComponent1; - msgId = msgId1; - // don't exist in mavlink1, so zero-them - incompat_flags = 0; - compat_flags = 0; - signature_len = 0; - // todo add more v1 here and don't just return - return; - } - if (magic != this.protocol_marker) { throw new Error("Invalid MAVLink prefix ("+magic+")"); } - // is packet supposed to be signed? - if ( incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED ){ - signature_len = mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; - } else { - signature_len = 0; - } - - // header's declared len compared to packets actual len - var actual_len = (msgbuf.length - (mavlink20.HEADER_LEN + 2 + signature_len)); - var actual_len_nosign = (msgbuf.length - (mavlink20.HEADER_LEN + 2 )); - - if ((mlen == actual_len) && (signature_len > 0)){ - var len_if_signed = mlen+signature_len; - //console.log("Packet appears signed && labeled as signed, OK. msgId=" + msgId); - - } else if ((mlen == actual_len_nosign) && (signature_len > 0)){ - - var len_if_signed = mlen+signature_len; + // is packet supposed to be signed? + if ( incompat_flags & mavlink20.MAVLINK_IFLAG_SIGNED ){ + signature_len = mavlink20.MAVLINK_SIGNATURE_BLOCK_LEN; + } else { + signature_len = 0; + } + + // header's declared len compared to packets actual len + var actual_len = (msgbuf.length - (header_len + 2 + signature_len)); + var actual_len_nosign = (msgbuf.length - (header_len + 2 )); + + if ((mlen == actual_len) && (signature_len > 0)){ + var len_if_signed = mlen+signature_len; + //console.log("Packet appears signed && labeled as signed, OK. msgId=" + msgId); + + } else if ((mlen == actual_len_nosign) && (signature_len > 0)){ + + var len_if_signed = mlen+signature_len; throw new Error("Packet appears unsigned when labeled as signed. Got actual_len "+actual_len_nosign+" expected " + len_if_signed + ", msgId=" + msgId); - - } else if( mlen != actual_len) { - throw new Error("Invalid MAVLink message length. Got " + (msgbuf.length - (mavlink20.HEADER_LEN + 2)) + " expected " + mlen + ", msgId=" + msgId); + + } else if( mlen != actual_len) { + throw new Error("Invalid MAVLink message length. Got " + (msgbuf.length - (header_len + 2)) + " expected " + mlen + ", msgId=" + msgId); } @@ -18675,83 +19361,81 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { throw new Error("Unknown MAVLink message ID (" + msgId + ")"); } - // here's the common chunks of packet we want to work with below.. - var headerBuf= msgbuf.slice(mavlink20.HEADER_LEN); // first10 - var sigBuf = msgbuf.slice(-signature_len); // last 13 or nothing - var crcBuf1 = msgbuf.slice(-2); // either last-2 or last-2-prior-to-signature - var crcBuf2 = msgbuf.slice(-15,-13); // either last-2 or last-2-prior-to-signature - var payloadBuf = msgbuf.slice(mavlink20.HEADER_LEN, -(signature_len+2)); // the remaining bit between the header and the crc - var crcCheckBuf = msgbuf.slice(1, -(signature_len+2)); // the part uses to calculate the crc - ie between the magic and signature, + // here's the common chunks of packet we want to work with below.. + var payloadBuf = msgbuf.slice(mavlink20.HEADER_LEN, -(signature_len+2)); // the remaining bit between the header and the crc + var crcCheckBuf = msgbuf.slice(1, -(signature_len+2)); // the part uses to calculate the crc - ie between the magic and signature, // decode the payload // refs: (fmt, type, order_map, crc_extra) = mavlink20.map[msgId] var decoder = mavlink20.map[msgId]; // decode the checksum - var receivedChecksum = undefined; - if ( signature_len == 0 ) { // unsigned - try { - receivedChecksum = jspack.Unpack(' payloadBuf.length) { payloadBuf = this.concat_buffer(payloadBuf, new Uint8Array(paylen - payloadBuf.length).fill(0)); } @@ -18822,18 +19506,18 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { // construct the message object try { // args at this point might look like: { '0': 6, '1': 8, '2': 0, '3': 0, '4': 3, '5': 3 } - var m = new decoder.type; // make a new 'empty' instance of the right class, + var m = new decoder.type(); // make a new 'empty' instance of the right class, m.set(args,false); // associate ordered-field-numbers to names, after construction not during. } catch (e) { throw new Error('Unable to instantiate MAVLink message of type '+decoder.type+' : ' + e.message); } - m._signed = sig_ok; - if (m._signed) { m._link_id = msgbuf[-13]; } + m._signed = sig_ok; + if (m._signed) { m._link_id = msgbuf[-13]; } m._msgbuf = msgbuf; - m._payload = payloadBuf + m._payload = payloadBuf; m.crc = receivedChecksum; m._header = new mavlink20.header(msgId, mlen, seq, srcSystem, srcComponent, incompat_flags, compat_flags); this.log(m); @@ -18841,8 +19525,22 @@ MAVLink20Processor.prototype.decode = function(msgbuf) { } -// allow loading as both common.js (Node), and/or vanilla javascript in-browser -if(typeof module === "object" && module.exports) { - module.exports = {mavlink20, MAVLink20Processor}; +// Browser and Node.js compatible module exports +if (!isNode) { + // For browsers, attach to window or use global namespace + if (typeof window !== 'undefined') { + window.mavlink20 = mavlink20; + window.MAVLink20Processor = MAVLink20Processor; + } + // Also support global assignment + if (typeof global !== 'undefined') { + global.mavlink20 = mavlink20; + global.MAVLink20Processor = MAVLink20Processor; + } +} else { + // For Node.js, use module.exports + if (typeof module === "object" && module.exports) { + module.exports = {mavlink20, MAVLink20Processor}; + } }