seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

feat(controls): Tilt-compensated compass heading node (Issue #235) #236

Merged
sl-jetson merged 2 commits from sl-controls/issue-235-compass into main 2026-03-02 12:15:02 -05:00
Conversation 0 Commits 2 Files Changed 12 +1045
sl-controls commented 2026-03-02 12:11:48 -05:00
Collaborator
Copy Link

Summary

Implement magnetometer-based heading calculation with tilt compensation and magnetic declination correction.

Features:

  • Tilt-compensated heading using quaternion-based orientation (roll/pitch compensation)
  • Magnetic declination correction for France: 1.5° east
  • Dual publishing: Float64 (degrees) + quaternion (Z-axis rotation)
  • 10Hz frequency with configurable parameters

Algorithm

  1. Subscribe to IMU (quaternion orientation) and magnetometer data
  2. Convert quaternion to roll/pitch/yaw for tilt compensation
  3. Project magnetometer onto horizontal plane using trig functions
  4. Apply declination: heading_true = heading_magnetic + 1.5°
  5. Normalize to 0-360° range
  6. Publish as Float64 degrees + QuaternionStamped

Tilt Compensation

  • Roll compensation: Tilting left/right affects east component
  • Pitch compensation: Tilting forward/backward affects north component
  • Full 3D: Handles combined tilts on uneven terrain

Test Coverage

✅ 30+ unit tests covering:

  • Node initialization and parameter configuration
  • Quaternion ↔ Euler conversion (identity, 90° rotations)
  • Heading quaternion creation and normalization
  • Tilt-compensated heading (roll, pitch, combined)
  • Declination correction and angle wrapping
  • Sensor subscription handlers
  • Realistic scenarios (level, tilted, 3D, weak signals)

Topics

Subscribed:

  • /saltybot/imu/data (sensor_msgs/Imu) - Orientation + optional mag
  • /saltybot/mag (sensor_msgs/MagneticField) - Magnetometer data

Published:

  • /saltybot/heading (std_msgs/Float64) - Heading in degrees (0-360)
  • /saltybot/heading_quaternion (geometry_msgs/QuaternionStamped) - Heading as Z-axis quaternion

Closes Issue #235.

## Summary Implement magnetometer-based heading calculation with tilt compensation and magnetic declination correction. **Features:** - **Tilt-compensated heading** using quaternion-based orientation (roll/pitch compensation) - **Magnetic declination correction** for France: 1.5° east - **Dual publishing**: Float64 (degrees) + quaternion (Z-axis rotation) - **10Hz frequency** with configurable parameters ## Algorithm 1. Subscribe to IMU (quaternion orientation) and magnetometer data 2. Convert quaternion to roll/pitch/yaw for tilt compensation 3. Project magnetometer onto horizontal plane using trig functions 4. Apply declination: heading_true = heading_magnetic + 1.5° 5. Normalize to 0-360° range 6. Publish as Float64 degrees + QuaternionStamped ## Tilt Compensation - **Roll compensation**: Tilting left/right affects east component - **Pitch compensation**: Tilting forward/backward affects north component - **Full 3D**: Handles combined tilts on uneven terrain ## Test Coverage ✅ 30+ unit tests covering: - Node initialization and parameter configuration - Quaternion ↔ Euler conversion (identity, 90° rotations) - Heading quaternion creation and normalization - Tilt-compensated heading (roll, pitch, combined) - Declination correction and angle wrapping - Sensor subscription handlers - Realistic scenarios (level, tilted, 3D, weak signals) ## Topics **Subscribed:** - `/saltybot/imu/data` (sensor_msgs/Imu) - Orientation + optional mag - `/saltybot/mag` (sensor_msgs/MagneticField) - Magnetometer data **Published:** - `/saltybot/heading` (std_msgs/Float64) - Heading in degrees (0-360) - `/saltybot/heading_quaternion` (geometry_msgs/QuaternionStamped) - Heading as Z-axis quaternion Closes Issue #235.
sl-controls added 2 commits 2026-03-02 12:11:48 -05:00 
Add real-time audio level visualization with VU-style meter:
- Responsive VU bar with color gradient (silent to clipping)
- Peak hold indicator with exponential decay
- Speech activity detection from /social/speech/is_speaking
- Color-coded audio levels with visual feedback
- Grid markers for level reference (25%, 50%, 75%)
- Comprehensive audio statistics (average, max, peak count)

Features:
- Dynamic color coding: Gray (silent) → Red (clipping)
- Level thresholds: Silent, Low, Moderate, Good, Loud, Clipping
- Peak hold with 1-second hold time + 5% decay per 50ms
- Speech activity indicator with pulsing animation
- 100-sample rolling average for statistics
- Real-time metric updates

Visual Elements:
- Main VU bar with smooth fill animation
- Separate peak hold display with glow effect
- Color reference legend (all 6 levels)
- Statistics panel (average, max, peak holds)
- Grid-based scale (0-100%)
- Speech status badge (SPEAKING/SILENT)

Integration:
- Added to SOCIAL tab as new "Audio" tab
- Subscribes to /saltybot/audio_level and /social/speech/is_speaking
- Properly formatted topic info footer
- Responsive design matching dashboard theme

Build: ✓ Passing (113 modules, 202.67 KB main bundle)

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
Implement magnetometer-based heading calculation with tilt compensation
and magnetic declination correction for France (1.5° east).

Features:
- Tilt-compensated heading using quaternion-based orientation
  Roll and pitch compensation for uneven terrain
- Magnetic declination correction: 1.5° for France
- Heading normalization to 0-360 degree range
- Publishes both Float64 (degrees) and quaternion representations
- 10Hz publishing frequency with configurable parameters

Algorithm:
- Subscribe to IMU (quaternion orientation) and magnetometer data
- Convert quaternion to roll/pitch/yaw for tilt compensation
- Project magnetometer vector onto horizontal plane using trig functions
- Apply declination correction and normalize heading
- Publish heading as Float64 degrees and quaternion (Z-axis rotation only)

Test Coverage:
- 30+ unit tests covering:
  - Node initialization and parameters
  - Quaternion to Euler conversion (identity, 90° rotations)
  - Heading quaternion creation (0°, 90°, 180°, custom angles)
  - Tilt-compensated heading with roll, pitch, combined tilts
  - Declination correction application
  - Sensor subscription handlers
  - Heading angle normalization and wrapping
  - Realistic scenarios (level, tilted uphill/sideways, 3D tilt, weak signal, continuous rotation)

Topics:
- Subscribed: /saltybot/imu/data (Imu), /saltybot/mag (MagneticField)
- Published: /saltybot/heading (Float64), /saltybot/heading_quaternion (QuaternionStamped)

Config: frequency=10Hz, declination_deg=1.5

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
sl-jetson merged commit 718c68e74d into main 2026-03-02 12:15:02 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#236
No description provided.
Delete Branch "sl-controls/issue-235-compass"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?