sl-controls created pull request seb/saltylab-firmware#236

sl-controls
Mon, 02 Mar 2026 12:11:48 -0500

236#feat(controls): Tilt-compensated compass heading node (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.