Magnetometer: correct field calculation (#2460)

- The magnetic intensity table has units centi gauss not centi tesla
- Add conversion to publish field in telsa.
- Field is calculated in NED frame.
- Convert to ENU frame for Gazebo world frame convention.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Author: srmainwaring

src/systems/magnetometer/Magnetometer.cc

@@ -98,7 +98,7 @@ static constexpr const int8_t inclination_table[13][37] = \
   { 71, 71, 72, 73, 75, 77, 78, 80, 81, 81, 80, 79, 77, 76, 74, 73, 73, 73, 73, 73, 73, 74, 74, 75, 76, 77, 78, 78, 78, 78, 77, 75, 73, 72, 71, 71, 71 },  // NOLINT
 };
 
-// strength data in centi-Tesla
+// strength data in centi gauss
 static constexpr const int8_t strength_table[13][37] = \
 {
   { 62, 60, 58, 56, 54, 52, 49, 46, 43, 41, 38, 36, 34, 32, 31, 31, 30, 30, 30, 31, 33, 35, 38, 42, 46, 51, 55, 59, 62, 64, 66, 67, 67, 66, 65, 64, 62 },  // NOLINT
@@ -134,6 +134,12 @@ class gz::sim::systems::MagnetometerPrivate
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
+  /// \brief True if the magnetic field is reported in gauss rather than tesla.
+  public: bool useUnitsGauss = true;
+
+  /// \brief True if the magnetic field earth frame is NED rather than ENU.
+  public: bool useEarthFrameNED = true;
+
   /// \brief Create sensor
   /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
@@ -226,7 +232,7 @@ class gz::sim::systems::MagnetometerPrivate
     return get_table_data(lat, lon, inclination_table);
   }
 
-  // return magnetic field strength in centi-Tesla
+  // return magnetic field strength in centi-Gauss
   float get_mag_strength(float lat, float lon)
   {
     return get_table_data(lat, lon, strength_table);
@@ -242,6 +248,29 @@ Magnetometer::Magnetometer() : System(), dataPtr(
 //////////////////////////////////////////////////
 Magnetometer::~Magnetometer() = default;
 
+//////////////////////////////////////////////////
+void Magnetometer::Configure(const Entity &/*_entity*/,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &/*_ecm*/,
+                           EventManager &/*_eventMgr*/)
+{
+    if (_sdf->HasElement("use_units_gauss"))
+    {
+      this->dataPtr->useUnitsGauss = _sdf->Get<bool>("use_units_gauss");
+    }
+    gzdbg << "Magnetometer: using param [use_units_gauss: "
+          << this->dataPtr->useUnitsGauss << "]."
+          << std::endl;
+
+    if (_sdf->HasElement("use_earth_frame_ned"))
+    {
+      this->dataPtr->useEarthFrameNED = _sdf->Get<bool>("use_earth_frame_ned");
+    }
+    gzdbg << "Magnetometer: using param [use_earth_frame_ned: "
+          << this->dataPtr->useEarthFrameNED << "]."
+          << std::endl;
+}
+
 //////////////////////////////////////////////////
 void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
@@ -446,16 +475,32 @@ void MagnetometerPrivate::Update(
             get_mag_inclination(
               lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI) * GZ_PI / 180;
 
-          // Magnetic strength (10^5xnanoTesla)
+          // Magnetic strength in gauss (10^5 nano tesla = 10^-2 centi gauss)
           float strength_ga =
             0.01f *
             get_mag_strength(lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI);
 
-          // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php
-          float H = strength_ga * cosf(inclination_rad);
-          float Z = tanf(inclination_rad) * H;
-          float X = H * cosf(declination_rad);
-          float Y = H * sinf(declination_rad);
+          // Magnetic intensity measured in telsa
+          float strength_tesla = 1.0E-4 * strength_ga;
+
+          // Magnetic field components are calculated in world NED frame using:
+          // http://geomag.nrcan.gc.ca/mag_fld/comp-en.php
+          float H = cosf(inclination_rad);
+          H *= this->useUnitsGauss ? strength_ga : strength_tesla;
+          float Z_ned = tanf(inclination_rad) * H;
+          float X_ned = H * cosf(declination_rad);
+          float Y_ned = H * sinf(declination_rad);
+
+          float X = X_ned;
+          float Y = Y_ned;
+          float Z = Z_ned;
+          if (!this->useEarthFrameNED)
+          {
+            // Use ENU convention for earth frame.
+            X = Y_ned;
+            Y = X_ned;
+            Z = -1.0 * Z_ned;
+          }
 
           math::Vector3d magnetic_field_I(X, Y, Z);
           it->second->SetWorldMagneticField(magnetic_field_I);
@@ -494,6 +539,7 @@ void MagnetometerPrivate::RemoveMagnetometerEntities(
 }
 
 GZ_ADD_PLUGIN(Magnetometer, System,
+  Magnetometer::ISystemConfigure,
   Magnetometer::ISystemPreUpdate,
   Magnetometer::ISystemPostUpdate
 )

src/systems/magnetometer/Magnetometer.hh

@@ -37,6 +37,7 @@ namespace systems
   /// current location.
   class Magnetometer:
     public System,
+    public ISystemConfigure,
     public ISystemPreUpdate,
     public ISystemPostUpdate
   {
@@ -46,6 +47,12 @@ namespace systems
     /// \brief Destructor
     public: ~Magnetometer() override;
 
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) override;
+
     /// Documentation inherited
     public: void PreUpdate(const UpdateInfo &_info,
                            EntityComponentManager &_ecm) final;