MagnetometerUtils.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "SensorDefs.hpp"
 
 
// *INDENT-OFF*
namespace MagnetometerUtils
{
    inline float dmsToDecimalDegrees(int degrees, int minutes, float seconds = 0.0f)
    {
        float sign = (degrees < 0 || (degrees == 0 && (minutes < 0 || seconds < 0))) ? -1.0f : 1.0f;
        return sign * (abs(degrees) + minutes / 60.0f + seconds / 3600.0f);
    }
 
    inline float dmsToRadians(int degrees, int minutes, float seconds = 0.0f)
    {
        return dmsToDecimalDegrees(degrees, minutes, seconds) * M_PI / 180.0f;
    }
 
    inline float calculateHeading(const MagnetometerData& data, float declination = 0.0f)
    {
        const auto& mag = data.magnetic_field;
 
        // Handle edge case when magnetic field is zero or aligned vertically
        if (mag.x == 0.0f && mag.y == 0.0f) {
            return 0.0f;
        }
 
        // Calculate angle from x-axis (east) using arctangent of y/x
        // atan2 returns angle in range [-π, π]
        float heading = atan2f(mag.y, mag.x);
 
        // Convert to [0, 2π) range (0 to 2π radians)
        if (heading < 0) {
            heading += 2 * M_PI;
        }
 
        // Apply magnetic declination correction
        // Positive declination adds to heading (east declination)
        heading += declination;
 
        // Normalize to [0, 2π) range after declination adjustment
        if (heading > 2 * M_PI) {
            heading -= 2 * M_PI;
        } else if (heading < 0) {
            heading += 2 * M_PI;
        }
 
        return heading;
    }
 
    inline float calculateHeadingDegrees(const MagnetometerData& data, float declination_deg = 0.0f)
    {
        const auto& mag = data.magnetic_field;
        if (mag.x == 0.0f && mag.y == 0.0f) {
            return 0.0f;
        }
        float heading_deg = atan2f(mag.y, mag.x) * 180.0f / M_PI;
        if (heading_deg < 0) {
            heading_deg += 360.0f;
        }
        heading_deg += declination_deg;
        if (heading_deg >= 360.0f) {
            heading_deg -= 360.0f;
        } else if (heading_deg < 0) {
            heading_deg += 360.0f;
        }
        return heading_deg;
    }
 
    inline float calculateMagneticStrength(const MagnetometerData& data)
    {
        const auto& mag = data.magnetic_field;
 
        // Compute Euclidean norm of the magnetic field vector
        return sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
    }
 
    inline float gaussToMicroTesla(float gauss)
    {
        return gauss * 100.0f; // 1 Gauss = 100 μT
    }
 
    inline float microTeslaToGauss(float ut)
    {
        return ut * 0.01f; // 1 μT = 0.01 Gauss
    }
 
 
    inline float rangeToGauss(MagFullScaleRange range)
    {
        switch (range) {
        case MagFullScaleRange::FS_4G:
            return 4.0f;
        case MagFullScaleRange::FS_8G:
            return 8.0f;
        case MagFullScaleRange::FS_12G:
            return 12.0f;
        case MagFullScaleRange::FS_16G:
            return 16.0f;
        case MagFullScaleRange::FS_25G:
            return 25.0f;
        case MagFullScaleRange::FS_30G:
            return 30.0f;
        case MagFullScaleRange::FS_32G:
            return 32.0f;
        default:
            return 0.0f;
        }
        return 0.0f;
    }
}
// *INDENT-ON*