Skip to content

Conversation

KrugD
Copy link

@KrugD KrugD commented Apr 25, 2025

Summary

Improves parsing and publishing of MAG_CAL_REPORT (MAVLink message ID 192) in the mag_calibration plugin.
This update aligns the MagnetometerReporter.msg structure with the official MAVLink documentation and ensures complete and accurate calibration report transmission via ROS 2 topic.

Details

  • Updated MagnetometerReporter.msg to include all fields defined in MAG_CAL_REPORT, such as:
    • fitness (calibration quality metric)
    • Offset: ofs_x, ofs_y, ofs_z
    • Diagonal: diag_x, diag_y, diag_z
    • Off-diagonal: offdiag_x, offdiag_y, offdiag_z
    • orientation_confidence, old_orientation, new_orientation
    • scale_factor, report, autosaved, cal_mask, etc.
  • Modified mag_calibration_status.cpp to:
    • Assign each field correctly from incoming MAVLink MAG_CAL_REPORT message
    • Construct full MagnetometerReporter ROS 2 message with all available data
  • This ensures the topic /mavros/mag_calibration/report contains accurate and complete information after compass calibration procedures.

Testing

  • ✅ Tested with ArduPilot SITL and real flight controller (Pixhawk) with ArduPilot firmware (Copter 4.4).
  • ✅ Calibration command sent using ros2 service call /mavros/cmd/command with:
  ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{
    command: 42424,
    confirmation: 1,
    param3: 1
  }"

✅ Observed output on ROS 2 topic:

ros2 topic echo /mavros/mag_calibration/report

📉 Before Fix (partial/incomplete output):

header:
  stamp:
    sec: 1742818332
    nanosec: 990819562
  frame_id: '0'
report: 4
confidence: 1.1955742835998535

📈 After Fix (fully populated according to MAVLink spec)

header:
  stamp:
    sec: 1745584691
    nanosec: 236922544
  frame_id: '0'
report: 4
confidence: 7.774540424346924
compass_id: 0
cal_mask: 1
cal_status: 4
autosaved: 1
fitness: 31.877994537353516
ofs_x: -43.93415451049805
ofs_y: -17.019399642944336
ofs_z: -22.630168914794922
diag_x: 1.0128146409988403
diag_y: 1.1006752252578735
diag_z: 0.9747106432914734
offdiag_x: 0.0010097220074385405
offdiag_y: -0.06996173411607742
offdiag_z: 0.056884389370679855
orientation_confidence: 7.774540424346924
old_orientation: 0
new_orientation: 0
scale_factor: 0.0

Motivation

Without this fix, the /mavros/mag_calibration/report topic published incomplete messages lacking critical calibration feedback fields like fitness, offsets, or orientation metrics.
This caused limited visibility for developers and GCS tools using MAVROS in ROS 2 setups.

With this PR, the message now reflects the full MAVLink MAG_CAL_REPORT, improving integration and diagnostics.

Copy link
Member

@vooon vooon left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM, Thanks!

@vooon
Copy link
Member

vooon commented Apr 26, 2025

Could you please apply ament_uncrustify from Jazzy or Rolling?

@KrugD
Copy link
Author

KrugD commented Apr 28, 2025

Good afternoon, I have completed everything. I have applied ament_uncrustify according to the standards from Rolling.

@vooon vooon merged commit c6fcf24 into mavlink:ros2 Apr 30, 2025
2 of 4 checks passed
@vooon vooon added this to the Version 2.10 milestone Apr 30, 2025
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
Status: TODO
Development

Successfully merging this pull request may close these issues.

2 participants