Depth sensor integration

app.py

@@ -8,6 +8,7 @@
 from lib.camera import init_camera, init_ip_camera, init_rpi_camera
 from lib.control_telemetry import init_control_telemetry
 from lib.controller import Controller
+from lib.depth_receiver import DepthTelemetryReceiver
 from lib.json_data_handler import JSONDataHandler
 from lib.log_udp_receiver import init_log_stream
 from lib.net_transport import DEFAULT_ROV_HOST
@@ -28,8 +29,15 @@
 app.config["CONTROLLER"] = Controller(bitmask_client=app.config["BITMASK"], rate_hz=60.0)
 app.config["CONTROLLER"].start()
 
-# Start background IMU receiver (UDP port 5002)
-app.config["IMU"] = init_imu_receiver(port=5002)
+# Start background sensor receiver (UDP port 5002). IMU and depth are separate
+# components, but the MCU currently sends both in the same JSON datagram.
+_sensor_data = JSONDataHandler()
+app.config["DEPTH"] = DepthTelemetryReceiver(data_handler=_sensor_data)
+app.config["IMU"] = init_imu_receiver(
+    port=5002,
+    data_handler=_sensor_data,
+    depth_receiver=app.config["DEPTH"],
+)
 
 # Load saved IMU axis mapping from config
 _config = JSONDataHandler(file_path=data_path("config.json"))

lib/depth_receiver.py

@@ -0,0 +1,62 @@
+from __future__ import annotations
+
+from typing import Any
+
+from lib.json_data_handler import JSONDataHandler
+
+
+class DepthTelemetryReceiver:
+    """Normalizes MS5837 depth telemetry from MCU sensor JSON packets."""
+
+    def __init__(self, data_handler=None):
+        self.data_handler = data_handler or JSONDataHandler()
+        self._last_depth = {}
+
+    def process_payload(self, depth: Any) -> dict | None:
+        if not isinstance(depth, dict) or not depth:
+            return None
+
+        depth_update = normalize_depth_payload(depth)
+        self._last_depth = depth_update
+        self.data_handler.update_data({"depth": depth_update})
+        return depth_update
+
+    def get_latest(self) -> dict:
+        return self._last_depth.copy()
+
+
+def normalize_depth_payload(depth: dict) -> dict:
+    return {
+        "dpt": _coerce_json_number(_depth_val(depth, "dpt")),
+        "dptSet": _coerce_json_number(_depth_val(depth, "dptSet")),
+        "pressure_mbar": _coerce_json_number(_depth_val(depth, "pressure_mbar"), precision=1),
+        "temperature_c": _coerce_json_number(_depth_val(depth, "temperature_c")),
+        "valid": _coerce_json_bool(depth.get("valid", False)),
+        "age_ms": _coerce_json_number(_depth_val(depth, "age_ms"), precision=0),
+        "addr": _coerce_json_number(_depth_val(depth, "addr"), precision=0),
+        "last_error": _coerce_json_number(_depth_val(depth, "last_error"), precision=0),
+        "init_attempts": _coerce_json_number(_depth_val(depth, "init_attempts"), precision=0),
+        "read_errors": _coerce_json_number(_depth_val(depth, "read_errors"), precision=0),
+    }
+
+
+def _depth_val(depth: dict, key: str, default: float = float("nan")) -> float:
+    value = depth.get(key, default)
+    try:
+        return float(value)
+    except (TypeError, ValueError):
+        return default
+
+
+def _coerce_json_number(value: float, precision: int = 2) -> Any:
+    if value != value:  # NaN check
+        return None
+    return round(float(value), precision)
+
+
+def _coerce_json_bool(value: Any) -> bool:
+    if isinstance(value, bool):
+        return value
+    if isinstance(value, str):
+        return value.strip().lower() in {"1", "true", "yes", "on"}
+    return bool(value)

lib/ninedof_receiver.py

@@ -6,6 +6,7 @@
 import time
 from typing import Any
 
+from lib.depth_receiver import DepthTelemetryReceiver
 from lib.json_data_handler import JSONDataHandler
 from lib.runtime_paths import log_path, logs_dir
 
@@ -40,10 +41,11 @@ def _build_remap(axes_cfg, valid_keys=("yaw", "pitch", "roll")):
 class IMUReceiver:
     """Background UDP receiver for VN-100S IMU data (yaw/pitch/roll) from Nucleo board."""
 
-    def __init__(self, host=UDP_IP, port=UDP_PORT, data_handler=None):
+    def __init__(self, host=UDP_IP, port=UDP_PORT, data_handler=None, depth_receiver=None):
         self.host = host
         self.port = port
         self.data_handler = data_handler or JSONDataHandler()
+        self.depth_receiver = depth_receiver or DepthTelemetryReceiver(data_handler=self.data_handler)
 
         self._stop = threading.Event()
         self._thread = threading.Thread(target=self._run, name="IMUReceiver", daemon=True)
@@ -172,6 +174,8 @@ def _process_packet(self, data: bytes, addr: tuple):
         self._log_raw_packet(text)
 
         imu = msg.get("imu", {})
+        if not isinstance(imu, dict):
+            imu = {}
 
         def _val(key: str, default: float = float("nan")) -> float:
             value = imu.get(key, default)
@@ -243,6 +247,11 @@ def _val(key: str, default: float = float("nan")) -> float:
         except Exception as e:
             print(f"IMU: Error updating data: {e}")
 
+        try:
+            self.depth_receiver.process_payload(msg.get("depth"))
+        except Exception as e:
+            print(f"Depth: Error updating data: {e}")
+
     def _log_raw_packet(self, text: str) -> None:
         try:
             with IMU_LOG.open("a", encoding="utf-8") as fp:
@@ -257,8 +266,8 @@ def _coerce_json_number(value: float, precision: int = 2) -> Any:
     return round(float(value), precision)
 
 
-def init_imu_receiver(host=UDP_IP, port=UDP_PORT, data_handler=None) -> IMUReceiver:
+def init_imu_receiver(host=UDP_IP, port=UDP_PORT, data_handler=None, depth_receiver=None) -> IMUReceiver:
     """Initialize and start the IMU receiver."""
-    receiver = IMUReceiver(host=host, port=port, data_handler=data_handler)
+    receiver = IMUReceiver(host=host, port=port, data_handler=data_handler, depth_receiver=depth_receiver)
     receiver.start()
     return receiver

routes.py

@@ -576,20 +576,20 @@ def start_pid_hold():
             return jsonify({"ok": False, "error": "Current attitude is incomplete"}), 503
 
         neutral = _neutralize_thruster_command()
-        setpoints = {**neutral, **attitude_setpoints}
         try:
             client.clear_override()
-            state = client.send_override(setpoints, replay_attempts=5, replay_delay=0.1)
+            state = client.send_override(attitude_setpoints, replay_attempts=5, replay_delay=0.1)
         except Exception as exc:  # pylint: disable=broad-except
             client.set_error(str(exc))
             return jsonify({"ok": False, "error": str(exc), "neutralized": True}), 503
 
         return jsonify(
             {
                 "ok": True,
-                "setpoints": setpoints,
+                "setpoints": attitude_setpoints,
                 "state": state,
                 "neutralized": True,
+                "manual_override": neutral,
                 "units": "deg",
             }
         )

static/css/pilot.css

@@ -173,7 +173,7 @@
   align-items: flex-start;
   gap: 12px;
 }
-.hud-depth { min-width: 120px; }
+.hud-depth { min-width: 140px; }
 .hud-heading {
   flex: 0 1 280px;
   text-align: center;

static/css/styles.css

@@ -111,6 +111,9 @@ body {
   color: #0dcaf0;
   font-weight: bold;
 }
+.custom-card .text-muted {
+  color: #adb5bd !important;
+}
 
 /* Custom Table */
 .custom-table {

static/js/depth.js

@@ -3,7 +3,33 @@ async function updateDepth() {
         const response = await fetch("/api/depth");
         const data = await response.json();
         const depthStatus = document.getElementById("depth-status");
-        depthStatus.textContent = `Depth: ${data.dpt}m / Target: ${data.dptSet}m`;
+        const depthTarget = document.getElementById("depth-target");
+        const depthTemperature = document.getElementById("depth-temperature");
+        const depthHealth = document.getElementById("depth-health");
+        const depth = Number.parseFloat(data.dpt);
+        const target = Number.parseFloat(data.dptSet);
+        const temperature = Number.parseFloat(data.temperature_c);
+
+        if (depthStatus) {
+            depthStatus.textContent = Number.isFinite(depth) ? `${depth.toFixed(2)} m` : "--.-- m";
+        }
+        if (depthTarget) {
+            depthTarget.textContent = Number.isFinite(target) ? `${target.toFixed(2)} m` : "-";
+        }
+        if (depthTemperature) {
+            depthTemperature.textContent = Number.isFinite(temperature) ? `${temperature.toFixed(2)} °C` : "--.-- °C";
+        }
+        if (depthHealth) {
+            const addr = Number(data.addr);
+            const addrText = Number.isFinite(addr) && addr > 0 ? `0x${addr.toString(16).padStart(2, "0")}` : "--";
+            if (data.valid) {
+                depthHealth.textContent = `VALID age ${data.age_ms ?? "--"} ms addr ${addrText}`;
+                depthHealth.className = "text-success mt-auto";
+            } else {
+                depthHealth.textContent = `INVALID err ${data.last_error ?? "--"} tries ${data.init_attempts ?? "--"} addr ${addrText}`;
+                depthHealth.className = "text-warning mt-auto";
+            }
+        }
     } catch (error) {
         console.error("Error fetching depth:", error);
     }

static/js/pilot.js

@@ -143,8 +143,16 @@
       const d = await res.json();
       const el = document.getElementById("hud-depth");
       const tgt = document.getElementById("hud-depth-target");
+      const temp = document.getElementById("hud-depth-temp");
+      const health = document.getElementById("hud-depth-health");
       if (el) el.textContent = d.dpt != null ? parseFloat(d.dpt).toFixed(1) : "--.-";
       if (tgt) tgt.textContent = d.dptSet != null ? parseFloat(d.dptSet).toFixed(1) : "--.-";
+      if (temp) temp.textContent = d.temperature_c != null ? parseFloat(d.temperature_c).toFixed(1) : "--.-";
+      if (health) {
+        const addr = Number(d.addr);
+        const addrText = Number.isFinite(addr) && addr > 0 ? `0x${addr.toString(16).padStart(2, "0")}` : "--";
+        health.textContent = d.valid ? `VALID ${d.age_ms ?? "--"}ms ${addrText}` : `ERR ${d.last_error ?? "--"} ${addrText}`;
+      }
     } catch (_) { /* silent */ }
   }
 

static/templates/layout.html

@@ -21,7 +21,9 @@ <h5 class="card-title">Battery</h5>
       <div class="card-body d-flex flex-column">
         <h5 class="card-title">Depth</h5>
         <p id="depth-status" class="display-6">Loading...</p>
-        <small class="text-muted mt-auto">Target: <span id="depth-target">-</span></small>
+        <small class="text-muted">Target: <span id="depth-target">-</span></small>
+        <small class="text-muted">Temp: <span id="depth-temperature">--.-- °C</span></small>
+        <small id="depth-health" class="text-muted mt-auto">Waiting for telemetry...</small>
       </div>
     </div>
   </div>

static/templates/pilot.html

@@ -31,6 +31,10 @@
         <div class="hud-sub">
           TGT <span id="hud-depth-target">--.-</span>m
         </div>
+        <div class="hud-sub">
+          TEMP <span id="hud-depth-temp">--.-</span>°C
+        </div>
+        <div class="hud-sub" id="hud-depth-health">NO DATA</div>
       </div>
 
       <!--
@@ -171,4 +175,4 @@
 
 <link rel="stylesheet" href="/static/css/pilot.css">
 <script src="/static/js/pilot.js"></script>
-{% endblock %}
+{% endblock %}