Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Magnetometer: correct field calculation #2460

Merged
merged 4 commits into from
Jul 8, 2024
Merged

Magnetometer: correct field calculation #2460

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c8bc12e
Magnetometer: correct field calculation
srmainwaring Jul 1, 2024
e8f1628
Magnetometer: add params for backwards compatibility
srmainwaring Jul 1, 2024
6603e5f
Magnetometer: fix code style
srmainwaring Jul 1, 2024
633c4e0
Magnetometer: remove include dependency on sdf/sdf.hh
srmainwaring Jul 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 54 additions & 8 deletions src/systems/magnetometer/Magnetometer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -494,6 +539,7 @@ void MagnetometerPrivate::RemoveMagnetometerEntities(
}

GZ_ADD_PLUGIN(Magnetometer, System,
Magnetometer::ISystemConfigure,
Magnetometer::ISystemPreUpdate,
Magnetometer::ISystemPostUpdate
)
Expand Down
7 changes: 7 additions & 0 deletions src/systems/magnetometer/Magnetometer.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ namespace systems
/// current location.
class Magnetometer:
public System,
public ISystemConfigure,
public ISystemPreUpdate,
public ISystemPostUpdate
{
Expand All @@ -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;
Expand Down
Loading