System Architecture =================== Overview -------- BlueROV SLAM is a **multi-sensor underwater localization system** combining: - **Dual side-scan sonar** (Kogger A/B) for seafloor imaging - **9-axis IMU** for attitude estimation - **Depth/pressure sensor** (MS5837) for depth - **High-precision temperature** (TSYS01) for CTD - **Conductivity sensor** (EZO EC) for salinity - **GPS** (u-blox) for surface position reference - **MAVLink bridge** to ArduPilot/BlueOS Architecture Layers ------------------- The system is organized in 4 layers: Layer 4: Visualization & Control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Web Dashboard (ports 8099, 8100) - WebSocket clients (live data streams) - Kogger App (desktop sonar viewer) - MAVLink GCS (QGroundControl) Communication: HTTP/WebSocket/UDP/MAVLink Layer 3: Docker Services (Raspberry Pi) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ **blueos-extension** (port 8099): - Binary protocol decoder (AA55) - JSON frame builder - WebSocket broadcaster (:8766) - UDP Kogger forwarder (:14555/:14556) - MAVLink bridge (:14401) - JSONL logger **kogger-viewer** (port 8100): - BB55 protocol parser - Ping assembler (multi-chunk echograms) - Waterfall display WebSocket (:8101) - Command forwarder (Pi → ESP32 → Kogger) Layer 2: Firmware (ESP32-WROOM-32UE) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Multi-sensor reader (tight polling loop) - Binary protocol encoder (AA55 frames) - Kogger compression (4:1 amplitude decimation) - Command relay (Pi → Kogger bidirectional) Layer 1: Sensors ^^^^^^^^^^^^^^^^ - Kogger A/B (side-scan sonar, BB55 protocol) - HWT905 IMU (WitMotion binary) - MS5837 (I2C depth/pressure) - TSYS01 (I2C temperature) - EZO EC (conductivity/salinity) - u-blox GPS (NMEA) Data Flow — End to End ----------------------- 1. Sensor Acquisition (ESP32) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ **Kogger Sonar** (921600 baud, binary BB55): - Read chunks from UART1 (A) and UART2 (B) - Compress echogram: 200 samples → 50 samples (4:1 decimation) - Bundle multiple chunks into single ``TYPE_KOG_COMPRESSED`` frame **IMU HWT905** (9600 baud, binary): - Parse 0x55 packets (accel, gyro, angle) - Extract roll/pitch/yaw (±180°, 0.01° resolution) **MS5837 Depth** (I2C 0x76): - Non-blocking ADC conversion (20ms delay per D1/D2) - Calculate pressure (mbar) and temperature (°C) **TSYS01 Temperature** (I2C 0x77): - High-precision temp (±0.015°C) **EZO EC Conductivity** (9600 baud): - Read conductivity (µS/cm) and salinity (PSU) **GPS u-blox** (9600 baud, NMEA): - TinyGPS++ parser for lat/lon/alt/satellites 2. Binary Protocol Decoder (blueos-extension) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ **Serial reader thread:** - Read USB serial @ 115200 baud - Parse AA55 frames with CRC validation - Dispatch by frame type **Sensor struct** (76 bytes, little-endian packed): .. code-block:: c struct SensorFrame { uint32_t ts; // millis() int16_t imu_roll; // ±18000 (0.01° scale) int16_t imu_pitch; int16_t imu_yaw; int16_t imu_ax, imu_ay, imu_az; // ±16000 (0.001g scale) uint32_t imu_pkts; uint16_t imu_errs; uint8_t imu_alive; int32_t gps_lat, gps_lon; // degE7 int16_t gps_alt; // 0.1m uint8_t gps_sat, gps_alive; uint16_t ec_value; // 0.1 µS/cm uint16_t ec_salinity; // 0.01 PSU uint8_t ec_alive; uint8_t pad[7]; // alignment uint16_t depth_press; // 0.1 mbar int16_t depth_temp; // 0.01 °C uint8_t depth_ok; int16_t celsius_temp; // 0.01 °C uint8_t celsius_ok; uint16_t sound_vel; // 0.1 m/s (Mackenzie 1981) uint16_t svp_depth; // 0.01 m uint8_t svp_sal; // 0.1 PSU uint32_t ka_pkts, ka_bytes, kb_pkts, kb_bytes; }; **JSON frame output:** .. code-block:: json { "ts": 12345, "imu": {"r": 1.5, "p": -2.3, "y": 180.0, "ax": 1.0, "ay": 0.0, "az": 0.0, "alive": true}, "gps": {"lat": 47.6123, "lon": -2.7456, "alt": 5.0, "sat": 8, "alive": true}, "ec": {"conductivity": 42000.0, "salinity": 35.5, "alive": true}, "depth": {"press": 1013.25, "temp": 15.2, "ok": true}, "celsius": {"temp": 15.18, "ok": true}, "ctd": {"sv": 1485.7, "d": 0.5, "s": 35.5, "t": 15.2}, "ka": {"pkts": 420, "bytes": 28000, "alive": true}, "kb": {"pkts": 380, "bytes": 32000, "alive": true} } 3. MAVLink Bridge ^^^^^^^^^^^^^^^^^ Sends to ArduPilot/BlueOS @ ``192.168.2.2:14401``: - **ATTITUDE (#30):** roll/pitch/yaw from IMU (radians) - **SCALED_PRESSURE (#29):** depth sensor (mbar, cdegC) - **GPS_RAW_INT (#24):** GPS lat/lon/alt (degE7, mm) MAVLink v2 encoding (no pymavlink dependency): - Manual message packing with CRC_EXTRA - X.25 CRC checksum - SysID=1, CompID=196 (MAV_COMP_ID_USER1) 4. Kogger Viewer — BB55 Waterfall Display ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ **UDP listeners** (port 14555 for A, 14556 for B): - Receive BB55 frames from blueos-extension - Parse Chart/Echogram packets (mode=0x01, id=3) **Ping assembler logic** (matches KoggerApp IDBinChart): BB55 Chart payload (206 bytes):: [0:2] uint16 seqOffset — chunk position in fill buffer [2:4] uint16 sampleResol [4:6] uint16 absOffset [6:206] 200 bytes amplitude (8-bit per sample) Ping assembly rules: - seqOffset == 0 → new ping starts - Place chunk at seqOffset in fill buffer - Emit completed ping when next seqOffset == 0 arrives - Handle wrap-around (B channel often starts mid-ping) **Waterfall WebSocket output** (:8101): .. code-block:: json { "ch": "A", "amp": "base64_encoded_amplitude_200_bytes", "n": 1234 } Protocol Specifications ----------------------- AA55 Binary Protocol (ESP32 → Pi) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Frame structure:: Offset | Size | Field | Description -------|------|-------------|---------------------------------- 0 | 1 | SYNC1 | 0xAA 1 | 1 | SYNC2 | 0x55 2 | 1 | Type | 0x01=sensors, 0x04=kogger compressed 3 | 2 | Length | Big-endian (len_hi << 8 | len_lo) 5 | N | Payload | N = Length bytes 5+N | 1 | CRC | type ^ len_hi ^ len_lo ^ XOR(payload) **CRC calculation:** .. code-block:: c uint8_t crc = type ^ len_hi ^ len_lo; for (int i = 0; i < len; i++) crc ^= payload[i]; BB55 Kogger Protocol ^^^^^^^^^^^^^^^^^^^^ Frame structure:: Offset | Size | Field | Description -------|------|----------|------------------------------- 0 | 1 | SYNC1 | 0xBB 1 | 1 | SYNC2 | 0x55 2 | 1 | Route | Routing byte (usually 0) 3 | 1 | Mode | Content flags 4 | 1 | ID | Packet type (3=Chart/Echogram) 5 | 1 | Length | Payload length 6 | N | Payload | N bytes 6+N | 2 | CRC | Fletcher-16 checksum **Fletcher-16 CRC:** .. code-block:: c uint8_t s1 = 0, s2 = 0; uint8_t region[] = {route, mode, id, length, ...payload}; for (uint8_t b : region) { s1 = (s1 + b) % 255; s2 = (s2 + s1) % 255; } crc_lo = s1; crc_hi = s2; Performance ----------- .. list-table:: :header-rows: 1 * - Metric - Value * - Sensor rate - 2Hz (50ms poll interval ESP32) * - Kogger throughput - ~50KB/s per channel @ 921600 baud * - Compression - 4:1 (200 → 50 samples) — 75% reduction * - USB serial - 115200 baud — ~10-11 KB/s effective * - Latency - <100ms (sensor → dashboard display) Future SLAM Pipeline -------------------- 1. **Dead reckoning:** IMU integration + depth → position estimate 2. **Scan matching:** Kogger dual-channel echograms → cross-correlation → drift correction 3. **EKF fusion:** IMU + sonar corrections → optimal state estimate 4. **Loop closure:** Feature matching when revisiting known area 5. **Bathymetric map:** Real-time seafloor reconstruction Technology Stack ---------------- - **ESP32:** Arduino framework, PlatformIO - **Pi services:** Python 3.11 (asyncio, websockets, pyserial) - **Docker:** BlueOS extension system - **Protocols:** Binary custom (AA55, BB55), MAVLink v2 - **Web:** HTML5 Canvas, WebSocket, vanilla JS References ---------- - `KoggerApp source `_ — BB55 protocol reference (IDBinChart) - `MAVLink v2 spec `_ - `BlueOS Extensions `_