Magnetometer system publishes field strength in incorrect units

[gilborty]

Environment

• OS Version:

$ lsb_release -a                 
LSB Version:	core-11.1.0ubuntu4-noarch:security-11.1.0ubuntu4-noarch
Distributor ID:	Ubuntu
Description:	Ubuntu 22.04.3 LTS
Release:	22.04
Codename:	jammy
---
$ uname -a                
Linux <> 6.5.0-17-generic #17~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Tue Jan 16 14:32:32 UTC 2 x86_64 x86_64 x86_64 GNU/Linux

• Source or binary build?

Binary
---
$ gz sim --version                                         
Gazebo Sim, version 8.0.0
Copyright (C) 2018 Open Source Robotics Foundation.
Released under the Apache 2.0 License.
---
$ ls /usr/include/gz
cmake3   fuel_tools9  launch7  msgs10    plugin2     sdformat14  sim8         utils2
common5  gui8         math7    physics7  rendering8  sensors8    transport13

Description

Expected behavior

According to the gz::msgs::Magnetometer proto message, the field_tesla is reported in units of Tesla.

However, consider the following minimal world file (see attached) using the Bay Area, California as the origin for the spherical_coordinates.

Using this website, I would expect the local magnetic field to be something like this:

Horizontal Intensity	North Comp	East Comp	Vertical Comp	Total Field
23,071.9 nT	22,485.8 nT	5,167.1 nT	41,506.4 nT	47,487.8 nT
2.3072×10^-5 T	2.2486×10^-5 T	5.1671×10^-6 T	4.15064×10^-5 T	4.74878×10^-5 T

However, when I start up the simulation and echo the /magnetometer topic

gz sim simple_mag.world
gz topic -e -t /magnetometer

I get the following:

header {
  stamp {
    sec: 67
    nsec: 230000000
  }
  data {
    key: "frame_id"
    value: "sensors_box::link::magnetometer"
  }
  data {
    key: "seq"
    value: "86"
  }
}
field_tesla {
  x: -0.32109555007132251
  y: -0.14981328784703124
  z: 0.40261160054926087
}

which is a few orders of magnitude off from what I was expecting.

I did some digging, and it looks like the tables that were used to estimate the field vector were added to gz-sim in this PR which actually use the field strength table from the old PX4 Sim.

However, I believe this line is incorrectly calculating the magnetic field in Tesla as I would expect the field to be measured in. Instead, it calculates it as a gauss and never converts it back to teslas.

I can demonstrate that this is the case using the previous message.

field_tesla {
  x: -0.32109555007132251
  y: -0.14981328784703124
  z: 0.40261160054926087 <--- Assume we are looking at the Z component only for this
}
---
Assume that it is actually gauss
0.40261160054926087 g = 4.0261160054926087×10^-5 T
4.0261160054926087×10^-5 T = 40261.160054926087 nT
40,261.2 nT ~= 41,506.4 nT (vertical component from NOAA website)

Steps to reproduce

1. gz sim simple_mag.world
2. gz topic -e -t /magnetometer
3. Observe a very high magnetic field strength (assuming field_tesla is in Teslas)

Unless I am mistaken, I think this is the case. My recommendation would be to fix the conversion in systems/magnetometer/Magnetometer.cc since we assume that the noise in the SDF file is measured in teslas. I think people would be okay with reporting it in tesla, not nT, even though that is what most sensors are reporting.

As an aside, I'm actually surprised this seems like it was incorrect all the way back to the PX4 days. Was it originally reporting the field as Gauss? It isn't clear from the message signature

I can put up a PR if you'd all like and agree with the approach. Please let me know if I missed anything.

[gilborty]

World file mentioned above (since Github doesn't like me uploading .world files)

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="simple_mag">
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>37.3861</latitude_deg>
      <longitude_deg>-122.0839</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    
    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="gz-sim-physics-system"
      name="gz::sim::systems::Physics">
    </plugin>
    <plugin
      filename="gz-sim-magnetometer-system"
      name="gz::sim::systems::Magnetometer">
    </plugin>

    <model name="sensors_box">
      <pose>4 0 3.0 0 0.0 3.14</pose>
      <link name="link">
        <pose>0.05 0.05 0.05 0 0 0</pose>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </visual>

        <sensor name="magnetometer" type="magnetometer">
          <always_on>1</always_on>
          <update_rate>100</update_rate>
          <visualize>true</visualize>
          <topic>magnetometer</topic>
          <enable_metrics>true</enable_metrics>
          <magnetometer>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>0.1</stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>0.1</stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>0.1</stddev>
              </noise>
            </z>
          </magnetometer>
        </sensor>

      </link>
    </model>
  </world>
</sdf>

[azeey]

Looking at the PX4 PR from git blame, I'm convinced that the unit there was gauss (https://github.com/EliaTarasov/PX4-SITL_gazebo-classic/blob/67e1dda2ec339b50085f7793abdacdee2c05d53a/include/gazebo_mavlink_interface.h#L307). I'd be in favor of fixing this, but since it's been a year since #1907 was merged, we shouldn't break existing behavior in gz-sim7 and gz-sim8. We should add a parameter to the MagnetoMeter system to enable the fix and enable it by default in main.

[azeey]

@gilborty Are you planning to create a PR for this? Is it okay if I assign this issue to you?

[gilborty]

Yes I should be able to get a PR up. To be clear, we would like a boolean parameter to enable the fix, right? And the units should be in tesla (as the message prescribes)?

[azeey]

To be clear, we would like a boolean parameter to enable the fix, right? And the units should be in tesla (as the message prescribes)?

Yes to both.

cc @ahcorde