worldmagmodel.cpp

/**
 ******************************************************************************
 *
 * @file       worldmagmodel.cpp
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Utilities to find the location of openpilot GCS files:
 *             - Plugins Share directory path
 *
 * @brief      Source file for the World Magnetic Model
 *             This is a port of code available from the US NOAA.
 *
 *             The hard coded coefficients should be valid until 2015.
 *
 *             Updated coeffs from ..
 *             http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml
 *
 *             NASA C source code ..
 *             http://www.ngdc.noaa.gov/geomag/WMM/wmm_wdownload.shtml
 *
 *             Major changes include:
 *                - No geoid model (altitude must be geodetic WGS-84)
 *                - Floating point calculation (not double precision)
 *                - Hard coded coefficients for model
 *                - Elimination of user interface
 *                - Elimination of dynamic memory allocation
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "worldmagmodel.h"

#include <stdint.h>
#include <QDebug>
#include <math.h>
#include <qmath.h>

#define RAD2DEG(rad)   ((rad) * (180.0 / M_PI))
#define DEG2RAD(deg)   ((deg) * (M_PI / 180.0))

// updated coeffs available from http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml
const double CoeffFile[91][6] = {
    {0, 0, 0, 0, 0, 0},
    {1, 0, -29496.6, 0.0, 11.6, 0.0},
    {1, 1, -1586.3, 4944.4, 16.5, -25.9},
    {2, 0, -2396.6, 0.0, -12.1, 0.0},
    {2, 1, 3026.1, -2707.7, -4.4, -22.5},
    {2, 2, 1668.6, -576.1, 1.9, -11.8},
    {3, 0, 1340.1, 0.0, 0.4, 0.0},
    {3, 1, -2326.2, -160.2, -4.1, 7.3},
    {3, 2, 1231.9, 251.9, -2.9, -3.9},
    {3, 3, 634.0, -536.6, -7.7, -2.6},
    {4, 0, 912.6, 0.0, -1.8, 0.0},
    {4, 1, 808.9, 286.4, 2.3, 1.1},
    {4, 2, 166.7, -211.2, -8.7, 2.7},
    {4, 3, -357.1, 164.3, 4.6, 3.9},
    {4, 4, 89.4, -309.1, -2.1, -0.8},
    {5, 0, -230.9, 0.0, -1.0, 0.0},
    {5, 1, 357.2, 44.6, 0.6, 0.4},
    {5, 2, 200.3, 188.9, -1.8, 1.8},
    {5, 3, -141.1, -118.2, -1.0, 1.2},
    {5, 4, -163.0, 0.0, 0.9, 4.0},
    {5, 5, -7.8, 100.9, 1.0, -0.6},
    {6, 0, 72.8, 0.0, -0.2, 0.0},
    {6, 1, 68.6, -20.8, -0.2, -0.2},
    {6, 2, 76.0, 44.1, -0.1, -2.1},
    {6, 3, -141.4, 61.5, 2.0, -0.4},
    {6, 4, -22.8, -66.3, -1.7, -0.6},
    {6, 5, 13.2, 3.1, -0.3, 0.5},
    {6, 6, -77.9, 55.0, 1.7, 0.9},
    {7, 0, 80.5, 0.0, 0.1, 0.0},
    {7, 1, -75.1, -57.9, -0.1, 0.7},
    {7, 2, -4.7, -21.1, -0.6, 0.3},
    {7, 3, 45.3, 6.5, 1.3, -0.1},
    {7, 4, 13.9, 24.9, 0.4, -0.1},
    {7, 5, 10.4, 7.0, 0.3, -0.8},
    {7, 6, 1.7, -27.7, -0.7, -0.3},
    {7, 7, 4.9, -3.3, 0.6, 0.3},
    {8, 0, 24.4, 0.0, -0.1, 0.0},
    {8, 1, 8.1, 11.0, 0.1, -0.1},
    {8, 2, -14.5, -20.0, -0.6, 0.2},
    {8, 3, -5.6, 11.9, 0.2, 0.4},
    {8, 4, -19.3, -17.4, -0.2, 0.4},
    {8, 5, 11.5, 16.7, 0.3, 0.1},
    {8, 6, 10.9, 7.0, 0.3, -0.1},
    {8, 7, -14.1, -10.8, -0.6, 0.4},
    {8, 8, -3.7, 1.7, 0.2, 0.3},
    {9, 0, 5.4, 0.0, 0.0, 0.0},
    {9, 1, 9.4, -20.5, -0.1, 0.0},
    {9, 2, 3.4, 11.5, 0.0, -0.2},
    {9, 3, -5.2, 12.8, 0.3, 0.0},
    {9, 4, 3.1, -7.2, -0.4, -0.1},
    {9, 5, -12.4, -7.4, -0.3, 0.1},
    {9, 6, -0.7, 8.0, 0.1, 0.0},
    {9, 7, 8.4, 2.1, -0.1, -0.2},
    {9, 8, -8.5, -6.1, -0.4, 0.3},
    {9, 9, -10.1, 7.0, -0.2, 0.2},
    {10, 0, -2.0, 0.0, 0.0, 0.0},
    {10, 1, -6.3, 2.8, 0.0, 0.1},
    {10, 2, 0.9, -0.1, -0.1, -0.1},
    {10, 3, -1.1, 4.7, 0.2, 0.0},
    {10, 4, -0.2, 4.4, 0.0, -0.1},
    {10, 5, 2.5, -7.2, -0.1, -0.1},
    {10, 6, -0.3, -1.0, -0.2, 0.0},
    {10, 7, 2.2, -3.9, 0.0, -0.1},
    {10, 8, 3.1, -2.0, -0.1, -0.2},
    {10, 9, -1.0, -2.0, -0.2, 0.0},
    {10, 10, -2.8, -8.3, -0.2, -0.1},
    {11, 0, 3.0, 0.0, 0.0, 0.0},
    {11, 1, -1.5, 0.2, 0.0, 0.0},
    {11, 2, -2.1, 1.7, 0.0, 0.1},
    {11, 3, 1.7, -0.6, 0.1, 0.0},
    {11, 4, -0.5, -1.8, 0.0, 0.1},
    {11, 5, 0.5, 0.9, 0.0, 0.0},
    {11, 6, -0.8, -0.4, 0.0, 0.1},
    {11, 7, 0.4, -2.5, 0.0, 0.0},
    {11, 8, 1.8, -1.3, 0.0, -0.1},
    {11, 9, 0.1, -2.1, 0.0, -0.1},
    {11, 10, 0.7, -1.9, -0.1, 0.0},
    {11, 11, 3.8, -1.8, 0.0, -0.1},
    {12, 0, -2.2, 0.0, 0.0, 0.0},
    {12, 1, -0.2, -0.9, 0.0, 0.0},
    {12, 2, 0.3, 0.3, 0.1, 0.0},
    {12, 3, 1.0, 2.1, 0.1, 0.0},
    {12, 4, -0.6, -2.5, -0.1, 0.0},
    {12, 5, 0.9, 0.5, 0.0, 0.0},
    {12, 6, -0.1, 0.6, 0.0, 0.1},
    {12, 7, 0.5, 0.0, 0.0, 0.0},
    {12, 8, -0.4, 0.1, 0.0, 0.0},
    {12, 9, -0.4, 0.3, 0.0, 0.0},
    {12, 10, 0.2, -0.9, 0.0, 0.0},
    {12, 11, -0.8, -0.2, -0.1, 0.0},
    {12, 12, 0.0, 0.9, 0.1, 0.0}
};

namespace Utils {

    WorldMagModel::WorldMagModel()
    {
        Initialize();
    }

    int WorldMagModel::GetMagVector(double LLA[3], int Month, int Day, int Year, double Be[3])
    {
        double Lat = LLA[0];
        double Lon = LLA[1];
        double AltEllipsoid = LLA[2];

        // ***********
        // range check supplied params

        if (Lat <  -90) return -1;  // error
        if (Lat >   90) return -2;  // error

        if (Lon < -180) return -3;  // error
        if (Lon >  180) return -4;  // error

        // ***********

        WMMtype_CoordSpherical CoordSpherical;
        WMMtype_CoordGeodetic CoordGeodetic;
        WMMtype_GeoMagneticElements GeoMagneticElements;

        Initialize();

        CoordGeodetic.lambda = Lon;
        CoordGeodetic.phi = Lat;
        CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid;

        // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report
        GeodeticToSpherical(&CoordGeodetic, &CoordSpherical);

        if (DateToYear(Month, Day, Year) < 0)
            return -5;  // error

        // Compute the geoMagnetic field elements and their time change
        if (Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements) < 0)
            return -6;  // error

        // set the returned values
        Be[0] = GeoMagneticElements.X * 1e-2;
        Be[1] = GeoMagneticElements.Y * 1e-2;
        Be[2] = GeoMagneticElements.Z * 1e-2;

        // ***********