|
|
570ebd3d22
|
feat(controls): Tilt-compensated compass heading node (Issue #235)
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>
|
2026-03-02 12:11:37 -05:00 |
|
