rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Flight/Libraries: Updated to coding conventions

Browse Source 
find ./flight/Libraries/ \! \( -name '*~' -a -prune \) -type f    | xargs -I{} bash -c 'echo {}; dos2unix {}; gnuindent -npro -kr -i8 -ts8 -sob -ss -ncs -cp1 -il0 {};'

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1706 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-21 19:29:35 +00:00 committed by peabody124
parent df0ed37d29
commit 833e8428d2
7 changed files with 2016 additions and 1850 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

412
flight/Libraries/CoordinateConversions.c
View File

@ -1,187 +1,225 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file CoordinateConversions.c * @file CoordinateConversions.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief General conversions with different coordinate systems. * @brief General conversions with different coordinate systems.
* - all angles in deg * - all angles in deg
* - distances in meters * - distances in meters
* - altitude above WGS-84 elipsoid * - altitude above WGS-84 elipsoid
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include <math.h> #include <math.h>
#include <stdint.h> #include <stdint.h>
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#define RAD2DEG (180.0/M_PI) #define RAD2DEG (180.0/M_PI)
#define DEG2RAD (M_PI/180.0) #define DEG2RAD (M_PI/180.0)
// ****** convert Lat,Lon,Alt to ECEF ************ // ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(double LLA[3], double ECEF[3]){ void LLA2ECEF(double LLA[3], double ECEF[3])
const double a = 6378137.0; // Equatorial Radius {
const double e = 8.1819190842622e-2; // Eccentricity const double a = 6378137.0; // Equatorial Radius
double sinLat, sinLon, cosLat, cosLon; const double e = 8.1819190842622e-2; // Eccentricity
double N; double sinLat, sinLon, cosLat, cosLon;
double N;
sinLat=sin(DEG2RAD*LLA[0]);
sinLon=sin(DEG2RAD*LLA[1]); sinLat = sin(DEG2RAD * LLA[0]);
cosLat=cos(DEG2RAD*LLA[0]); sinLon = sin(DEG2RAD * LLA[1]);
cosLon=cos(DEG2RAD*LLA[1]); cosLat = cos(DEG2RAD * LLA[0]);
cosLon = cos(DEG2RAD * LLA[1]);
N = a / sqrt(1.0 - e*e*sinLat*sinLat); //prime vertical radius of curvature
N = a / sqrt(1.0 - e * e * sinLat * sinLat); //prime vertical radius of curvature
ECEF[0] = (N+LLA[2])*cosLat*cosLon;
ECEF[1] = (N+LLA[2])*cosLat*sinLon; ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
ECEF[2] = ((1-e*e)*N + LLA[2]) * sinLat; ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
} ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat;
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* }
uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
{ // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
const double a = 6378137.0; // Equatorial Radius uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
const double e = 8.1819190842622e-2; // Eccentricity {
double x=ECEF[0], y=ECEF[1], z=ECEF[2]; const double a = 6378137.0; // Equatorial Radius
double Lat, N, NplusH, delta, esLat; const double e = 8.1819190842622e-2; // Eccentricity
uint16_t iter; double x = ECEF[0], y = ECEF[1], z = ECEF[2];
#define MAX_ITER 100 double Lat, N, NplusH, delta, esLat;
uint16_t iter;
LLA[1] = RAD2DEG*atan2(y,x); #define MAX_ITER 100
N = a;
NplusH = N; LLA[1] = RAD2DEG * atan2(y, x);
delta = 1; N = a;
Lat = 1; NplusH = N;
iter=0; delta = 1;
Lat = 1;
while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < MAX_ITER)) iter = 0;
{
delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH)))); while (((delta > 1.0e-14) || (delta < -1.0e-14))
Lat = Lat-delta; && (iter < MAX_ITER)) {
esLat = e*sin(Lat); delta =
N = a / sqrt(1 - esLat*esLat); Lat -
NplusH = sqrt(x*x + y*y)/cos(Lat); atan(z /
iter += 1; (sqrt(x * x + y * y) *
} (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
LLA[0] = RAD2DEG*Lat; esLat = e * sin(Lat);
LLA[2] = NplusH - N; N = a / sqrt(1 - esLat * esLat);
NplusH = sqrt(x * x + y * y) / cos(Lat);
return (iter < MAX_ITER); iter += 1;
} }
// ****** find ECEF to NED rotation matrix ******** LLA[0] = RAD2DEG * Lat;
void RneFromLLA(double LLA[3], float Rne[3][3]){ LLA[2] = NplusH - N;
float sinLat, sinLon, cosLat, cosLon;
return (iter < MAX_ITER);
sinLat=(float)sin(DEG2RAD*LLA[0]); }
sinLon=(float)sin(DEG2RAD*LLA[1]);
cosLat=(float)cos(DEG2RAD*LLA[0]); // ****** find ECEF to NED rotation matrix ********
cosLon=(float)cos(DEG2RAD*LLA[1]); void RneFromLLA(double LLA[3], float Rne[3][3])
{
Rne[0][0] = -sinLat*cosLon; Rne[0][1] = -sinLat*sinLon; Rne[0][2] = cosLat; float sinLat, sinLon, cosLat, cosLon;
Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0;
Rne[2][0] = -cosLat*cosLon; Rne[2][1] = -cosLat*sinLon; Rne[2][2] = -sinLat; sinLat = (float)sin(DEG2RAD * LLA[0]);
} sinLon = (float)sin(DEG2RAD * LLA[1]);
cosLat = (float)cos(DEG2RAD * LLA[0]);
// ****** find roll, pitch, yaw from quaternion ******** cosLon = (float)cos(DEG2RAD * LLA[1]);
void Quaternion2RPY(float q[4], float rpy[3]){
float R13, R11, R12, R23, R33; Rne[0][0] = -sinLat * cosLon;
float q0s=q[0]*q[0]; Rne[0][1] = -sinLat * sinLon;
float q1s=q[1]*q[1]; Rne[0][2] = cosLat;
float q2s=q[2]*q[2]; Rne[1][0] = -sinLon;
float q3s=q[3]*q[3]; Rne[1][1] = cosLon;
Rne[1][2] = 0;
R13 = 2*(q[1]*q[3]-q[0]*q[2]); Rne[2][0] = -cosLat * cosLon;
R11 = q0s+q1s-q2s-q3s; Rne[2][1] = -cosLat * sinLon;
R12 = 2*(q[1]*q[2]+q[0]*q[3]); Rne[2][2] = -sinLat;
R23 = 2*(q[2]*q[3]+q[0]*q[1]); }
R33 = q0s-q1s-q2s+q3s;
// ****** find roll, pitch, yaw from quaternion ********
rpy[1]=RAD2DEG*asinf(-R13); // pitch always between -pi/2 to pi/2 void Quaternion2RPY(float q[4], float rpy[3])
rpy[2]=RAD2DEG*atan2f(R12,R11); {
rpy[0]=RAD2DEG*atan2f(R23,R33); float R13, R11, R12, R23, R33;
} float q0s = q[0] * q[0];
float q1s = q[1] * q[1];
// ****** find quaternion from roll, pitch, yaw ******** float q2s = q[2] * q[2];
void RPY2Quaternion(float rpy[3], float q[4]){ float q3s = q[3] * q[3];
float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi; R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
R11 = q0s + q1s - q2s - q3s;
phi=DEG2RAD*rpy[0]/2; theta=DEG2RAD*rpy[1]/2; psi=DEG2RAD*rpy[2]/2; R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
cphi=cosf(phi); sphi=sinf(phi); R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
ctheta=cosf(theta); stheta=sinf(theta); R33 = q0s - q1s - q2s + q3s;
cpsi=cosf(psi); spsi=sinf(psi);
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
q[0] = cphi*ctheta*cpsi + sphi*stheta*spsi; rpy[2] = RAD2DEG * atan2f(R12, R11);
q[1] = sphi*ctheta*cpsi - cphi*stheta*spsi; rpy[0] = RAD2DEG * atan2f(R23, R33);
q[2] = cphi*stheta*cpsi + sphi*ctheta*spsi; }
q[3] = cphi*ctheta*spsi - sphi*stheta*cpsi;
// ****** find quaternion from roll, pitch, yaw ********
if (q[0] < 0){ // q0 always positive for uniqueness void RPY2Quaternion(float rpy[3], float q[4])
q[0]=-q[0]; {
q[1]=-q[1]; float phi, theta, psi;
q[2]=-q[2]; float cphi, sphi, ctheta, stheta, cpsi, spsi;
q[3]=-q[3];
} phi = DEG2RAD * rpy[0] / 2;
} theta = DEG2RAD * rpy[1] / 2;
psi = DEG2RAD * rpy[2] / 2;
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** cphi = cosf(phi);
void Quaternion2R(float q[4], float Rbe[3][3]){ sphi = sinf(phi);
ctheta = cosf(theta);
float q0s=q[0]*q[0], q1s=q[1]*q[1], q2s=q[2]*q[2], q3s=q[3]*q[3]; stheta = sinf(theta);
cpsi = cosf(psi);
Rbe[0][0]=q0s+q1s-q2s-q3s; spsi = sinf(psi);
Rbe[0][1]=2*(q[1]*q[2]+q[0]*q[3]);
Rbe[0][2]=2*(q[1]*q[3]-q[0]*q[2]); q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi;
Rbe[1][0]=2*(q[1]*q[2]-q[0]*q[3]); q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi;
Rbe[1][1]=q0s-q1s+q2s-q3s; q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi;
Rbe[1][2]=2*(q[2]*q[3]+q[0]*q[1]); q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi;
Rbe[2][0]=2*(q[1]*q[3]+q[0]*q[2]);
Rbe[2][1]=2*(q[2]*q[3]-q[0]*q[1]); if (q[0] < 0) { // q0 always positive for uniqueness
Rbe[2][2]=q0s-q1s-q2s+q3s; q[0] = -q[0];
} q[1] = -q[1];
q[2] = -q[2];
// ****** Express LLA in a local NED Base Frame ******** q[3] = -q[3];
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]){ }
double ECEF[3]; }
float diff[3];
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
LLA2ECEF(LLA,ECEF); void Quaternion2R(float q[4], float Rbe[3][3])
{
diff[0]=(float)(ECEF[0]-BaseECEF[0]);
diff[1]=(float)(ECEF[1]-BaseECEF[1]); float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s =
diff[2]=(float)(ECEF[2]-BaseECEF[2]); q[2] * q[2], q3s = q[3] * q[3];
NED[0]= Rne[0][0]*diff[0]+Rne[0][1]*diff[1]+Rne[0][2]*diff[2]; Rbe[0][0] = q0s + q1s - q2s - q3s;
NED[1]= Rne[1][0]*diff[0]+Rne[1][1]*diff[1]+Rne[1][2]*diff[2]; Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
NED[2]= Rne[2][0]*diff[0]+Rne[2][1]*diff[1]+Rne[2][2]*diff[2]; Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
} Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
Rbe[1][1] = q0s - q1s + q2s - q3s;
// ****** Express ECEF in a local NED Base Frame ******** Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]){ Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
float diff[3]; Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
Rbe[2][2] = q0s - q1s - q2s + q3s;
diff[0]=(float)(ECEF[0]-BaseECEF[0]); }
diff[1]=(float)(ECEF[1]-BaseECEF[1]);
diff[2]=(float)(ECEF[2]-BaseECEF[2]); // ****** Express LLA in a local NED Base Frame ********
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3],
NED[0]= Rne[0][0]*diff[0]+Rne[0][1]*diff[1]+Rne[0][2]*diff[2]; float NED[3])
NED[1]= Rne[1][0]*diff[0]+Rne[1][1]*diff[1]+Rne[1][2]*diff[2]; {
NED[2]= Rne[2][0]*diff[0]+Rne[2][1]*diff[1]+Rne[2][2]*diff[2]; double ECEF[3];
} float diff[3];
LLA2ECEF(LLA, ECEF);
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
NED[0] =
Rne[0][0] * diff[0] + Rne[0][1] * diff[1] +
Rne[0][2] * diff[2];
NED[1] =
Rne[1][0] * diff[0] + Rne[1][1] * diff[1] +
Rne[1][2] * diff[2];
NED[2] =
Rne[2][0] * diff[0] + Rne[2][1] * diff[1] +
Rne[2][2] * diff[2];
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3],
float NED[3])
{
float diff[3];
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
NED[0] =
Rne[0][0] * diff[0] + Rne[0][1] * diff[1] +
Rne[0][2] * diff[2];
NED[1] =
Rne[1][0] * diff[0] + Rne[1][1] * diff[1] +
Rne[1][2] * diff[2];
NED[2] =
Rne[2][0] * diff[0] + Rne[2][1] * diff[1] +
Rne[2][2] * diff[2];
}

2222
flight/Libraries/WorldMagModel.c
View File

@ -1,1055 +1,1167 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file WorldMagModel.c * @file WorldMagModel.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Source file for the World Magnetic Model * @brief Source file for the World Magnetic Model
* This is a port of code available from the US NOAA. * This is a port of code available from the US NOAA.
* The hard coded coefficients should be valid until 2015. * The hard coded coefficients should be valid until 2015.
* Major changes include: * Major changes include:
* - No geoid model (altitude must be geodetic WGS-84) * - No geoid model (altitude must be geodetic WGS-84)
* - Floating point calculation (not double precision) * - Floating point calculation (not double precision)
* - Hard coded coefficients for model * - Hard coded coefficients for model
* - Elimination of user interface * - Elimination of user interface
* - Elimination of dynamic memory allocation * - Elimination of dynamic memory allocation
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
// I don't want this dependency, but currently using pvPortMalloc // I don't want this dependency, but currently using pvPortMalloc
#include "openpilot.h" #include "openpilot.h"
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdint.h> #include <stdint.h>
#include "WorldMagModel.h" #include "WorldMagModel.h"
#include "WMMInternal.h" #include "WMMInternal.h"
static WMMtype_Ellipsoid * Ellip; static WMMtype_Ellipsoid *Ellip;
static WMMtype_MagneticModel * MagneticModel; static WMMtype_MagneticModel *MagneticModel;
/************************************************************************************** /**************************************************************************************
* Example use - very simple - only two exposed functions * Example use - very simple - only two exposed functions
* *
* WMM_Initialize(); // Set default values and constants * WMM_Initialize(); // Set default values and constants
* *
* WMM_GetMagVector(float Lat, float Lon, float Alt, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]); * WMM_GetMagVector(float Lat, float Lon, float Alt, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]);
* e.g. Iceland in may of 2012 = WMM_GetMagVector(65.0, -20.0, 0.0, 5, 5, 2012, B); * e.g. Iceland in may of 2012 = WMM_GetMagVector(65.0, -20.0, 0.0, 5, 5, 2012, B);
* Alt is above the WGS-84 Ellipsoid * Alt is above the WGS-84 Ellipsoid
* B is the NED (XYZ) magnetic vector in nTesla * B is the NED (XYZ) magnetic vector in nTesla
**************************************************************************************/ **************************************************************************************/
int WMM_Initialize() int WMM_Initialize()
// Sets default values for WMM subroutines. // Sets default values for WMM subroutines.
// UPDATES : Ellip and MagneticModel // UPDATES : Ellip and MagneticModel
{ {
// Sets WGS-84 parameters // Sets WGS-84 parameters
Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km
Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km
Ellip->fla = 1/298.257223563; // flattening Ellip->fla = 1 / 298.257223563; // flattening
Ellip->eps = sqrt(1- (Ellip->b*Ellip->b)/(Ellip->a*Ellip->a )); // first eccentricity Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity
Ellip->epssq = (Ellip->eps*Ellip->eps); // first eccentricity squared Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared
Ellip->re = 6371.2; // Earth's radius in km Ellip->re = 6371.2; // Earth's radius in km
// Sets Magnetic Model parameters // Sets Magnetic Model parameters
MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; MagneticModel->nMax = WMM_MAX_MODEL_DEGREES;
MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; MagneticModel->nMaxSecVar =
MagneticModel->SecularVariationUsed = 0; WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES;
MagneticModel->SecularVariationUsed = 0;
// Really, Really needs to be read from a file - out of date in 2015 at latest
MagneticModel->EditionDate = 5.7863328170559505e-307; // Really, Really needs to be read from a file - out of date in 2015 at latest
MagneticModel->epoch = 2010.0; MagneticModel->EditionDate = 5.7863328170559505e-307;
sprintf(MagneticModel->ModelName, "WMM-2010"); MagneticModel->epoch = 2010.0;
WMM_Set_Coeff_Array(); sprintf(MagneticModel->ModelName, "WMM-2010");
return 0; WMM_Set_Coeff_Array();
} return 0;
}
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3])
{ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid,
char Error_Message[255]; uint16_t Month, uint16_t Day, uint16_t Year,
float B[3])
Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid)); {
MagneticModel = (WMMtype_MagneticModel *) pvPortMalloc(sizeof(WMMtype_MagneticModel)); char Error_Message[255];
WMMtype_CoordSpherical * CoordSpherical = (WMMtype_CoordSpherical *) pvPortMalloc(sizeof(CoordSpherical)); Ellip =
WMMtype_CoordGeodetic * CoordGeodetic = (WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic)); (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid));
WMMtype_Date * Date = (WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date)); MagneticModel =
WMMtype_GeoMagneticElements * GeoMagneticElements = (WMMtype_GeoMagneticElements *) pvPortMalloc(sizeof(GeoMagneticElements)); (WMMtype_MagneticModel *)
pvPortMalloc(sizeof(WMMtype_MagneticModel));
WMM_Initialize();
WMMtype_CoordSpherical *CoordSpherical =
CoordGeodetic->lambda = Lon; (WMMtype_CoordSpherical *)
CoordGeodetic->phi = Lat; pvPortMalloc(sizeof(CoordSpherical));
CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid; WMMtype_CoordGeodetic *CoordGeodetic =
WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/ (WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic));
WMMtype_Date *Date =
Date->Month=Month; (WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date));
Date->Day=Day; WMMtype_GeoMagneticElements *GeoMagneticElements =
Date->Year=Year; (WMMtype_GeoMagneticElements *)
WMM_DateToYear (Date, Error_Message); pvPortMalloc(sizeof(GeoMagneticElements));
WMM_TimelyModifyMagneticModel(Date);
WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/ WMM_Initialize();
B[0]=GeoMagneticElements->X; CoordGeodetic->lambda = Lon;
B[1]=GeoMagneticElements->Y; CoordGeodetic->phi = Lat;
B[2]=GeoMagneticElements->Z; CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid;
WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report */
vPortFree(Ellip);
vPortFree(MagneticModel); Date->Month = Month;
vPortFree(CoordSpherical); Date->Day = Day;
vPortFree(CoordGeodetic); Date->Year = Year;
vPortFree(Date); WMM_DateToYear(Date, Error_Message);
vPortFree(GeoMagneticElements); WMM_TimelyModifyMagneticModel(Date);
} WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change */
uint16_t WMM_Geomag( WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, B[0] = GeoMagneticElements->X;
WMMtype_GeoMagneticElements *GeoMagneticElements) B[1] = GeoMagneticElements->Y;
/* B[2] = GeoMagneticElements->Z;
The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point.
The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and vPortFree(Ellip);
their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid vPortFree(MagneticModel);
of magnetic field, these are better achieved by the subroutine WMM_Grid. vPortFree(CoordSpherical);
vPortFree(CoordGeodetic);
INPUT: Ellip vPortFree(Date);
CoordSpherical vPortFree(GeoMagneticElements);
CoordGeodetic }
TimedMagneticModel
uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
OUTPUT : GeoMagneticElements WMMtype_CoordGeodetic * CoordGeodetic,
WMMtype_GeoMagneticElements * GeoMagneticElements)
CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) /*
WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF The main subroutine that calls a sequence of WMM sub-functions to calculate the magnetic field elements for a single point.
WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients The function expects the model coefficients and point coordinates as input and returns the magnetic field elements and
WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid
WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates of magnetic field, these are better achieved by the subroutine WMM_Grid.
WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates
WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements INPUT: Ellip
WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements CoordSpherical
CoordGeodetic
*/ TimedMagneticModel
{
WMMtype_LegendreFunction LegendreFunction; OUTPUT : GeoMagneticElements
WMMtype_SphericalHarmonicVariables SphVariables;
WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar; CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables )
WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF
WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */ WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients
WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */ WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients
WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates
WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates
WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements
WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates*/ WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements
WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); /* Calculate the Geomagnetic elements, Equation 18 , WMM Technical report */
WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); /*Calculate the secular variation of each of the Geomagnetic elements*/ */
return TRUE; {
} WMMtype_LegendreFunction LegendreFunction;
WMMtype_SphericalHarmonicVariables SphVariables;
WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo,
uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables) MagneticResultsSphVar, MagneticResultsGeoVar;
/* Computes Spherical variables WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */
Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */
summations. (Equations 10-12 in the WMM Technical Report) WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients */
INPUT Ellip data structure with the following elements WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */
float a; semi-major axis of the ellipsoid WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */
float b; semi-minor axis of the ellipsoid WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates */
float fla; flattening WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); /* Calculate the Geomagnetic elements, Equation 18 , WMM Technical report */
float epssq; first eccentricity squared WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); /*Calculate the secular variation of each of the Geomagnetic elements */
float eps; first eccentricity return TRUE;
float re; mean radius of ellipsoid }
CoordSpherical A data structure with the following elements
float lambda; ( longitude) uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *
float phig; ( geocentric latitude ) CoordSpherical,
float r; ( distance from the center of the ellipsoid) uint16_t nMax,
nMax integer ( Maxumum degree of spherical harmonic secular model)\ WMMtype_SphericalHarmonicVariables
* SphVariables)
OUTPUT SphVariables Pointer to the data structure with the following elements
float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n /* Computes Spherical variables
float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude) Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic
float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude) summations. (Equations 10-12 in the WMM Technical Report)
CALLS : none INPUT Ellip data structure with the following elements
*/ float a; semi-major axis of the ellipsoid
float b; semi-minor axis of the ellipsoid
{ float fla; flattening
float cos_lambda, sin_lambda; float epssq; first eccentricity squared
uint16_t m, n; float eps; first eccentricity
cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); float re; mean radius of ellipsoid
sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); CoordSpherical A data structure with the following elements
/* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) float lambda; ( longitude)
for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ float phig; ( geocentric latitude )
SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); float r; ( distance from the center of the ellipsoid)
for (n = 1; n <= nMax; n++) nMax integer ( Maxumum degree of spherical harmonic secular model)\
{
SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n-1] * (Ellip->re / CoordSpherical->r); OUTPUT SphVariables Pointer to the data structure with the following elements
} float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; [earth_reference_radius_km sph. radius ]^n
float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; cp(m) - cosine of (mspherical coord. longitude)
/* float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; sp(m) - sine of (mspherical coord. longitude)
Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax CALLS : none
cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) */
sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) {
*/ float cos_lambda, sin_lambda;
SphVariables->cos_mlambda[0] = 1.0; uint16_t m, n;
SphVariables->sin_mlambda[0] = 0.0; cos_lambda = cos(DEG2RAD(CoordSpherical->lambda));
sin_lambda = sin(DEG2RAD(CoordSpherical->lambda));
SphVariables->cos_mlambda[1] = cos_lambda; /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2)
SphVariables->sin_mlambda[1] = sin_lambda; for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */
for (m = 2; m <= nMax; m++) SphVariables->RelativeRadiusPower[0] =
{ (Ellip->re / CoordSpherical->r) * (Ellip->re /
SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m-1]*cos_lambda - SphVariables->sin_mlambda[m-1]*sin_lambda; CoordSpherical->r);
SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m-1]*sin_lambda + SphVariables->sin_mlambda[m-1]*cos_lambda; for (n = 1; n <= nMax; n++) {
} SphVariables->RelativeRadiusPower[n] =
return TRUE; SphVariables->RelativeRadiusPower[n -
} /*WMM_ComputeSphericalHarmonicVariables*/ 1] * (Ellip->re /
CoordSpherical->
r);
uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction) }
/* Computes all of the Schmidt-semi normalized associated Legendre /*
functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used. Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax
Otherwise WMM_PcupHigh is called. cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b)
INPUT CoordSpherical A data structure with the following elements sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b)
float lambda; ( longitude) */
float phig; ( geocentric latitude ) SphVariables->cos_mlambda[0] = 1.0;
float r; ( distance from the center of the ellipsoid) SphVariables->sin_mlambda[0] = 0.0;
nMax integer ( Maxumum degree of spherical harmonic secular model)
LegendreFunction Pointer to data structure with the following elements SphVariables->cos_mlambda[1] = cos_lambda;
float *Pcup; ( pointer to store Legendre Function ) SphVariables->sin_mlambda[1] = sin_lambda;
float *dPcup; ( pointer to store Derivative of Lagendre function ) for (m = 2; m <= nMax; m++) {
SphVariables->cos_mlambda[m] =
OUTPUT LegendreFunction Calculated Legendre variables in the data structure SphVariables->cos_mlambda[m - 1] * cos_lambda -
SphVariables->sin_mlambda[m - 1] * sin_lambda;
*/ SphVariables->sin_mlambda[m] =
SphVariables->cos_mlambda[m - 1] * sin_lambda +
{ SphVariables->sin_mlambda[m - 1] * cos_lambda;
float sin_phi; }
uint16_t FLAG = 1; return TRUE;
} /*WMM_ComputeSphericalHarmonicVariables */
sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); /* sin (geocentric latitude) */
uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *
if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10 ) /* If nMax is less tha 16 or at the poles */ CoordSpherical, uint16_t nMax,
FLAG = WMM_PcupLow(LegendreFunction->Pcup,LegendreFunction->dPcup,sin_phi, nMax); WMMtype_LegendreFunction *
else FLAG = WMM_PcupHigh(LegendreFunction->Pcup,LegendreFunction->dPcup,sin_phi, nMax); LegendreFunction)
if (FLAG == 0) /* Error while computing Legendre variables*/
return FALSE; /* Computes all of the Schmidt-semi normalized associated Legendre
functions up to degree nMax. If nMax <= 16, function WMM_PcupLow is used.
Otherwise WMM_PcupHigh is called.
return TRUE; INPUT CoordSpherical A data structure with the following elements
} /*WMM_AssociatedLegendreFunction */ float lambda; ( longitude)
float phig; ( geocentric latitude )
uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) float r; ( distance from the center of the ellipsoid)
{ nMax integer ( Maxumum degree of spherical harmonic secular model)
/* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using LegendreFunction Pointer to data structure with the following elements
spherical harmonic summation. float *Pcup; ( pointer to store Legendre Function )
float *dPcup; ( pointer to store Derivative of Lagendre function )
The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential OUTPUT LegendreFunction Calculated Legendre variables in the data structure
The gradient in spherical coordinates is given by:
*/
dV ^ 1 dV ^ 1 dV ^ {
grad V = -- r + - -- t + -------- -- p float sin_phi;
dr r dt r sin(t) dp uint16_t FLAG = 1;
sin_phi = sin(DEG2RAD(CoordSpherical->phig)); /* sin (geocentric latitude) */
INPUT : LegendreFunction
MagneticModel if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */
SphVariables FLAG =
CoordSpherical WMM_PcupLow(LegendreFunction->Pcup,
OUTPUT : MagneticResults LegendreFunction->dPcup, sin_phi, nMax);
else
CALLS : WMM_SummationSpecial FLAG =
WMM_PcupHigh(LegendreFunction->Pcup,
LegendreFunction->dPcup, sin_phi, nMax);
if (FLAG == 0) /* Error while computing Legendre variables */
Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov return FALSE;
*/
uint16_t m, n, index; return TRUE;
float cos_phi; } /*WMM_AssociatedLegendreFunction */
MagneticResults->Bz = 0.0;
MagneticResults->By = 0.0; uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
MagneticResults->Bx = 0.0; WMMtype_SphericalHarmonicVariables * SphVariables,
for (n = 1; n <= MagneticModel->nMax; n++) WMMtype_CoordSpherical * CoordSpherical,
{ WMMtype_MagneticResults * MagneticResults)
for (m=0;m<=n;m++) {
{ /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using
index = (n * (n + 1) / 2 + m); spherical harmonic summation.
/* nMax (n+2) n m m m The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) The gradient in spherical coordinates is given by:
n=1 m=0 n n n */
/* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ dV ^ 1 dV ^ 1 dV ^
MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * grad V = -- r + - -- t + -------- -- p
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + dr r dt r sin(t) dp
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] )
* (float) (n+1) * LegendreFunction-> Pcup[index]; INPUT : LegendreFunction
MagneticModel
/* 1 nMax (n+2) n m m m SphVariables
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) CoordSpherical
n=1 m=0 n n n */ OUTPUT : MagneticResults
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * CALLS : WMM_SummationSpecial
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] -
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] ) Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov
* (float) (m) * LegendreFunction-> Pcup[index]; */
/* nMax (n+2) n m m m uint16_t m, n, index;
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) float cos_phi;
n=1 m=0 n n n */ MagneticResults->Bz = 0.0;
/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ MagneticResults->By = 0.0;
MagneticResults->Bx = 0.0;
MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * for (n = 1; n <= MagneticModel->nMax; n++) {
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + for (m = 0; m <= n; m++) {
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) index = (n * (n + 1) / 2 + m);
* LegendreFunction-> dPcup[index];
/* nMax (n+2) n m m m
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi))
n=1 m=0 n n n */
} /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/
} MagneticResults->Bz -=
SphVariables->RelativeRadiusPower[n] *
cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); (MagneticModel->Main_Field_Coeff_G[index] *
if ( fabs(cos_phi) > 1.0e-10 ) SphVariables->cos_mlambda[m] +
{ MagneticModel->Main_Field_Coeff_H[index] *
MagneticResults->By = MagneticResults->By / cos_phi ; SphVariables->sin_mlambda[m])
} * (float)(n +
else 1) * LegendreFunction->Pcup[index];
/* Special calculation for component - By - at Geographic poles.
* If the user wants to avoid using this function, please make sure that /* 1 nMax (n+2) n m m m
* the latitude is not exactly +/-90. An option is to make use the function By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
* WMM_CheckGeographicPoles. n=1 m=0 n n n */
*/ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By +=
{ SphVariables->RelativeRadiusPower[n] *
WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults); (MagneticModel->Main_Field_Coeff_G[index] *
} SphVariables->sin_mlambda[m] -
return TRUE; MagneticModel->Main_Field_Coeff_H[index] *
}/*WMM_Summation */ SphVariables->cos_mlambda[m])
* (float)(m) * LegendreFunction->Pcup[index];
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) /* nMax (n+2) n m m m
{ Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
/*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. n=1 m=0 n n n */
INPUT : LegendreFunction /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */
MagneticModel
SphVariables MagneticResults->Bx -=
CoordSpherical SphVariables->RelativeRadiusPower[n] *
OUTPUT : MagneticResults (MagneticModel->Main_Field_Coeff_G[index] *
SphVariables->cos_mlambda[m] +
CALLS : WMM_SecVarSummationSpecial MagneticModel->Main_Field_Coeff_H[index] *
SphVariables->sin_mlambda[m])
*/ * LegendreFunction->dPcup[index];
uint16_t m, n, index;
float cos_phi; }
MagneticModel->SecularVariationUsed = TRUE; }
MagneticResults->Bz = 0.0;
MagneticResults->By = 0.0; cos_phi = cos(DEG2RAD(CoordSpherical->phig));
MagneticResults->Bx = 0.0; if (fabs(cos_phi) > 1.0e-10) {
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) MagneticResults->By = MagneticResults->By / cos_phi;
{ } else
for (m=0;m<=n;m++) /* Special calculation for component - By - at Geographic poles.
{ * If the user wants to avoid using this function, please make sure that
index = (n * (n + 1) / 2 + m); * the latitude is not exactly +/-90. An option is to make use the function
* WMM_CheckGeographicPoles.
/* nMax (n+2) n m m m */
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi))
n=1 m=0 n n n */ {
/* Derivative with respect to radius.*/ WMM_SummationSpecial(SphVariables, CoordSpherical,
MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * MagneticResults);
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + }
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) return TRUE;
* (float) (n+1) * LegendreFunction-> Pcup[index]; } /*WMM_Summation */
/* 1 nMax (n+2) n m m m uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) WMMtype_SphericalHarmonicVariables *
n=1 m=0 n n n */ SphVariables,
/* Derivative with respect to longitude, divided by radius. */ WMMtype_CoordSpherical * CoordSpherical,
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * WMMtype_MagneticResults * MagneticResults)
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] - {
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] ) /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector.
* (float) (m) * LegendreFunction-> Pcup[index]; INPUT : LegendreFunction
/* nMax (n+2) n m m m MagneticModel
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) SphVariables
n=1 m=0 n n n */ CoordSpherical
/* Derivative with respect to latitude, divided by radius. */ OUTPUT : MagneticResults
MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * CALLS : WMM_SecVarSummationSpecial
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] +
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) */
* LegendreFunction-> dPcup[index]; uint16_t m, n, index;
} float cos_phi;
} MagneticModel->SecularVariationUsed = TRUE;
cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); MagneticResults->Bz = 0.0;
if ( fabs(cos_phi) > 1.0e-10 ) MagneticResults->By = 0.0;
{ MagneticResults->Bx = 0.0;
MagneticResults->By = MagneticResults->By / cos_phi ; for (n = 1; n <= MagneticModel->nMaxSecVar; n++) {
} for (m = 0; m <= n; m++) {
else index = (n * (n + 1) / 2 + m);
/* Special calculation for component By at Geographic poles */
{ /* nMax (n+2) n m m m
WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi))
} n=1 m=0 n n n */
return TRUE; /* Derivative with respect to radius.*/
} /*WMM_SecVarSummation*/ MagneticResults->Bz -=
SphVariables->RelativeRadiusPower[n] *
uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo) (MagneticModel->Secular_Var_Coeff_G[index] *
/* Rotate the Magnetic Vectors to Geodetic Coordinates SphVariables->cos_mlambda[m] +
Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov MagneticModel->Secular_Var_Coeff_H[index] *
Equation 16, WMM Technical report SphVariables->sin_mlambda[m])
* (float)(n +
INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements 1) * LegendreFunction->Pcup[index];
float lambda; ( longitude)
float phig; ( geocentric latitude ) /* 1 nMax (n+2) n m m m
float r; ( distance from the center of the ellipsoid) By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
n=1 m=0 n n n */
CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements /* Derivative with respect to longitude, divided by radius. */
float lambda; (longitude) MagneticResults->By +=
float phi; ( geodetic latitude) SphVariables->RelativeRadiusPower[n] *
float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) (MagneticModel->Secular_Var_Coeff_G[index] *
float HeightAboveGeoid;(height above the Geoid ) SphVariables->sin_mlambda[m] -
MagneticModel->Secular_Var_Coeff_H[index] *
MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements SphVariables->cos_mlambda[m])
float Bx; North * (float)(m) * LegendreFunction->Pcup[index];
float By; East /* nMax (n+2) n m m m
float Bz; Down Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
n=1 m=0 n n n */
OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements /* Derivative with respect to latitude, divided by radius. */
float Bx; North
float By; East MagneticResults->Bx -=
float Bz; Down SphVariables->RelativeRadiusPower[n] *
(MagneticModel->Secular_Var_Coeff_G[index] *
CALLS : none SphVariables->cos_mlambda[m] +
MagneticModel->Secular_Var_Coeff_H[index] *
*/ SphVariables->sin_mlambda[m])
{ * LegendreFunction->dPcup[index];
float Psi; }
/* Difference between the spherical and Geodetic latitudes */ }
Psi = ( M_PI/180 ) * ( CoordSpherical->phig - CoordGeodetic->phi ); cos_phi = cos(DEG2RAD(CoordSpherical->phig));
if (fabs(cos_phi) > 1.0e-10) {
/* Rotate spherical field components to the Geodeitic system */ MagneticResults->By = MagneticResults->By / cos_phi;
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); } else
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); /* Special calculation for component By at Geographic poles */
MagneticResultsGeo->By = MagneticResultsSph->By; {
return TRUE; WMM_SecVarSummationSpecial(SphVariables, CoordSpherical,
} /*WMM_RotateMagneticVector*/ MagneticResults);
}
uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) return TRUE;
} /*WMM_SecVarSummation */
/* Calculate all the Geomagnetic elements from X,Y and Z components
INPUT MagneticResultsGeo Pointer to data structure with the following elements uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
float Bx; ( North ) WMMtype_CoordGeodetic * CoordGeodetic,
float By; ( East ) WMMtype_MagneticResults *
float Bz; ( Down ) MagneticResultsSph,
OUTPUT GeoMagneticElements Pointer to data structure with the following elements WMMtype_MagneticResults *
float Decl; (Angle between the magnetic field vector and true north, positive east) MagneticResultsGeo)
float Incl; Angle between the magnetic field vector and the horizontal plane, positive down /* Rotate the Magnetic Vectors to Geodetic Coordinates
float F; Magnetic Field Strength Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov
float H; Horizontal Magnetic Field Strength Equation 16, WMM Technical report
float X; Northern component of the magnetic field vector
float Y; Eastern component of the magnetic field vector INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements
float Z; Downward component of the magnetic field vector float lambda; ( longitude)
CALLS : none float phig; ( geocentric latitude )
*/ float r; ( distance from the center of the ellipsoid)
{
GeoMagneticElements->X = MagneticResultsGeo->Bx; CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements
GeoMagneticElements->Y = MagneticResultsGeo->By; float lambda; (longitude)
GeoMagneticElements->Z = MagneticResultsGeo->Bz; float phi; ( geodetic latitude)
float HeightAboveEllipsoid; (height above the ellipsoid (HaE) )
GeoMagneticElements->H = sqrt (MagneticResultsGeo->Bx* MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); float HeightAboveGeoid;(height above the Geoid )
GeoMagneticElements->F = sqrt (GeoMagneticElements->H*GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
GeoMagneticElements->Decl = RAD2DEG(atan2 (GeoMagneticElements->Y , GeoMagneticElements->X)); MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements
GeoMagneticElements->Incl = RAD2DEG(atan2 (GeoMagneticElements->Z , GeoMagneticElements->H)); float Bx; North
float By; East
return TRUE; float Bz; Down
} /*WMM_CalculateGeoMagneticElements */
OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements
uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) float Bx; North
/*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. float By; East
INPUT MagneticVariation Data structure with the following elements float Bz; Down
float Bx; ( North )
float By; ( East ) CALLS : none
float Bz; ( Down )
OUTPUT MagneticElements Pointer to the data structure with the following elements updated */
float Decldot; Yearly Rate of change in declination {
float Incldot; Yearly Rate of change in inclination float Psi;
float Fdot; Yearly rate of change in Magnetic field strength /* Difference between the spherical and Geodetic latitudes */
float Hdot; Yearly rate of change in horizontal field strength Psi = (M_PI / 180) * (CoordSpherical->phig - CoordGeodetic->phi);
float Xdot; Yearly rate of change in the northern component
float Ydot; Yearly rate of change in the eastern component /* Rotate spherical field components to the Geodeitic system */
float Zdot; Yearly rate of change in the downward component MagneticResultsGeo->Bz =
float GVdot;Yearly rate of chnage in grid variation MagneticResultsSph->Bx * sin(Psi) +
CALLS : none MagneticResultsSph->Bz * cos(Psi);
MagneticResultsGeo->Bx =
*/ MagneticResultsSph->Bx * cos(Psi) -
{ MagneticResultsSph->Bz * sin(Psi);
MagneticElements->Xdot = MagneticVariation->Bx; MagneticResultsGeo->By = MagneticResultsSph->By;
MagneticElements->Ydot = MagneticVariation->By; return TRUE;
MagneticElements->Zdot = MagneticVariation->Bz; } /*WMM_RotateMagneticVector */
MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report
MagneticElements->Fdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F; uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *
MagneticElements->Decldot = 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); MagneticResultsGeo,
MagneticElements->Incldot = 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); WMMtype_GeoMagneticElements *
MagneticElements->GVdot = MagneticElements->Decldot; GeoMagneticElements)
return TRUE;
} /*WMM_CalculateSecularVariation*/ /* Calculate all the Geomagnetic elements from X,Y and Z components
INPUT MagneticResultsGeo Pointer to data structure with the following elements
uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) float Bx; ( North )
float By; ( East )
/* This function evaluates all of the Schmidt-semi normalized associated Legendre float Bz; ( Down )
functions up to degree nMax. The functions are initially scaled by OUTPUT GeoMagneticElements Pointer to data structure with the following elements
10^280 sin^m in order to minimize the effects of underflow at large m float Decl; (Angle between the magnetic field vector and true north, positive east)
near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). float Incl; Angle between the magnetic field vector and the horizontal plane, positive down
Note that this function performs the same operation as WMM_PcupLow. float F; Magnetic Field Strength
However this function also can be used for high degree (large nMax) models. float H; Horizontal Magnetic Field Strength
float X; Northern component of the magnetic field vector
Calling Parameters: float Y; Eastern component of the magnetic field vector
INPUT float Z; Downward component of the magnetic field vector
nMax: Maximum spherical harmonic degree to compute. CALLS : none
x: cos(colatitude) or sin(latitude). */
{
OUTPUT GeoMagneticElements->X = MagneticResultsGeo->Bx;
Pcup: A vector of all associated Legendgre polynomials evaluated at GeoMagneticElements->Y = MagneticResultsGeo->By;
x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. GeoMagneticElements->Z = MagneticResultsGeo->Bz;
dPcup: Derivative of Pcup(x) with respect to latitude
GeoMagneticElements->H =
CALLS : none sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx +
Notes: MagneticResultsGeo->By * MagneticResultsGeo->By);
GeoMagneticElements->F =
sqrt(GeoMagneticElements->H * GeoMagneticElements->H +
MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. GeoMagneticElements->Decl =
RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X));
Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov GeoMagneticElements->Incl =
RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H));
Change from the previous version
The prevous version computes the derivatives as return TRUE;
dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). } /*WMM_CalculateGeoMagneticElements */
However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude.
Hence the derivatives are multiplied by sin(latitude). uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *
Removed the options for CS phase and normalizations. MagneticVariation,
WMMtype_GeoMagneticElements *
Note: In geomagnetism, the derivatives of ALF are usually found with MagneticElements)
respect to the colatitudes. Here the derivatives are found with respect /*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements.
to the latitude. The difference is a sign reversal for the derivative of INPUT MagneticVariation Data structure with the following elements
the Associated Legendre Functions. float Bx; ( North )
float By; ( East )
The derivates can't be computed for latitude = |90| degrees. float Bz; ( Down )
*/ OUTPUT MagneticElements Pointer to the data structure with the following elements updated
{ float Decldot; Yearly Rate of change in declination
float pm2, pm1, pmm, plm, rescalem, z, scalef; float Incldot; Yearly Rate of change in inclination
float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP]; float Fdot; Yearly rate of change in Magnetic field strength
uint16_t k, kstart, m, n; float Hdot; Yearly rate of change in horizontal field strength
float Xdot; Yearly rate of change in the northern component
if (fabs(x) == 1.0) float Ydot; Yearly rate of change in the eastern component
{ float Zdot; Yearly rate of change in the downward component
// printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); float GVdot;Yearly rate of chnage in grid variation
return FALSE; CALLS : none
}
*/
scalef = 1.0e-280; {
MagneticElements->Xdot = MagneticVariation->Bx;
for(n = 0 ; n <= 2*nMax+1 ; ++n ) MagneticElements->Ydot = MagneticVariation->By;
{ MagneticElements->Zdot = MagneticVariation->Bz;
PreSqr[n] = sqrt((float)(n)); MagneticElements->Hdot = (MagneticElements->X * MagneticElements->Xdot + MagneticElements->Y * MagneticElements->Ydot) / MagneticElements->H; //See equation 19 in the WMM technical report
} MagneticElements->Fdot =
(MagneticElements->X * MagneticElements->Xdot +
k = 2; MagneticElements->Y * MagneticElements->Ydot +
MagneticElements->Z * MagneticElements->Zdot) /
for(n=2 ; n<=nMax ; n++) MagneticElements->F;
{ MagneticElements->Decldot =
k = k + 1; 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot -
f1[k] = (float)(2*n-1) /(float)(n); MagneticElements->Y * MagneticElements->Xdot) /
f2[k] = (float)(n-1) /(float)(n); (MagneticElements->H * MagneticElements->H);
for(m=1 ; m<=n-2 ; m++) MagneticElements->Incldot =
{ 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot -
k = k+1; MagneticElements->Z * MagneticElements->Hdot) /
f1[k] = (float)(2*n-1) / PreSqr[n+m] / PreSqr[n-m]; (MagneticElements->F * MagneticElements->F);
f2[k] = PreSqr[n-m-1] * PreSqr[n+m-1] / PreSqr[n+m] / PreSqr[n-m]; MagneticElements->GVdot = MagneticElements->Decldot;
} return TRUE;
k = k + 2; } /*WMM_CalculateSecularVariation */
}
uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
/*z = sin (geocentric latitude) */
z = sqrt((1.0-x)*(1.0+x)); /* This function evaluates all of the Schmidt-semi normalized associated Legendre
pm2 = 1.0; functions up to degree nMax. The functions are initially scaled by
Pcup[0] = 1.0; 10^280 sin^m in order to minimize the effects of underflow at large m
dPcup[0] = 0.0; near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299).
if (nMax == 0) Note that this function performs the same operation as WMM_PcupLow.
return FALSE; However this function also can be used for high degree (large nMax) models.
pm1 = x;
Pcup[1] = pm1; Calling Parameters:
dPcup[1] = z; INPUT
k = 1; nMax: Maximum spherical harmonic degree to compute.
x: cos(colatitude) or sin(latitude).
for(n = 2; n <= nMax; n++ )
{ OUTPUT
k = k+n; Pcup: A vector of all associated Legendgre polynomials evaluated at
plm = f1[k]*x*pm1-f2[k]*pm2; x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2.
Pcup[k] = plm; dPcup: Derivative of Pcup(x) with respect to latitude
dPcup[k] = (float)(n) * (pm1 - x * plm) / z;
pm2 = pm1; CALLS : none
pm1 = plm; Notes:
}
Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005.
pmm = PreSqr[2]*scalef;
rescalem = 1.0/scalef; Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov
kstart = 0;
Change from the previous version
for(m = 1; m <= nMax - 1; ++m) The prevous version computes the derivatives as
{ dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ).
rescalem = rescalem*z; However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude.
Hence the derivatives are multiplied by sin(latitude).
/* Calculate Pcup(m,m)*/ Removed the options for CS phase and normalizations.
kstart = kstart+m+1;
pmm = pmm * PreSqr[2*m+1] / PreSqr[2*m]; Note: In geomagnetism, the derivatives of ALF are usually found with
Pcup[kstart] = pmm*rescalem / PreSqr[2*m+1]; respect to the colatitudes. Here the derivatives are found with respect
dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); to the latitude. The difference is a sign reversal for the derivative of
pm2 = pmm/PreSqr[2*m+1]; the Associated Legendre Functions.
/* Calculate Pcup(m+1,m)*/
k = kstart+m+1 ; The derivates can't be computed for latitude = |90| degrees.
pm1 = x * PreSqr[2*m+1] * pm2; */
Pcup[k] = pm1*rescalem; {
dPcup[k] = ((pm2*rescalem) * PreSqr[2*m+1] - x * (float)(m+1) * Pcup[k]) / z; float pm2, pm1, pmm, plm, rescalem, z, scalef;
/* Calculate Pcup(n,m)*/ float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP];
for(n = m+2; n <= nMax; ++n) uint16_t k, kstart, m, n;
{
k = k+n; if (fabs(x) == 1.0) {
plm = x*f1[k]*pm1-f2[k]*pm2; // printf("Error in PcupHigh: derivative cannot be calculated at poles\n");
Pcup[k] = plm*rescalem; return FALSE;
dPcup[k] = (PreSqr[n+m] * PreSqr[n-m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k] ) / z; }
pm2 = pm1;
pm1 = plm; scalef = 1.0e-280;
}
} for (n = 0; n <= 2 * nMax + 1; ++n) {
PreSqr[n] = sqrt((float)(n));
/* Calculate Pcup(nMax,nMax)*/ }
rescalem = rescalem*z;
kstart = kstart+m+1; k = 2;
pmm = pmm / PreSqr[2*nMax];
Pcup[kstart] = pmm * rescalem; for (n = 2; n <= nMax; n++) {
dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; k = k + 1;
f1[k] = (float)(2 * n - 1) / (float)(n);
return TRUE ; f2[k] = (float)(n - 1) / (float)(n);
} /* WMM_PcupHigh */ for (m = 1; m <= n - 2; m++) {
k = k + 1;
uint16_t WMM_PcupLow( float *Pcup, float *dPcup, float x, uint16_t nMax) f1[k] =
(float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n -
/* This function evaluates all of the Schmidt-semi normalized associated Legendre m];
functions up to degree nMax. f2[k] =
PreSqr[n - m - 1] * PreSqr[n + m -
Calling Parameters: 1] / PreSqr[n +
INPUT m] /
nMax: Maximum spherical harmonic degree to compute. PreSqr[n - m];
x: cos(colatitude) or sin(latitude). }
k = k + 2;
OUTPUT }
Pcup: A vector of all associated Legendgre polynomials evaluated at
x up to nMax. /*z = sin (geocentric latitude) */
dPcup: Derivative of Pcup(x) with respect to latitude z = sqrt((1.0 - x) * (1.0 + x));
pm2 = 1.0;
Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. Pcup[0] = 1.0;
Use WMM_PcupHigh for large nMax. dPcup[0] = 0.0;
if (nMax == 0)
Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. return FALSE;
pm1 = x;
Note: In geomagnetism, the derivatives of ALF are usually found with Pcup[1] = pm1;
respect to the colatitudes. Here the derivatives are found with respect dPcup[1] = z;
to the latitude. The difference is a sign reversal for the derivative of k = 1;
the Associated Legendre Functions.
*/ for (n = 2; n <= nMax; n++) {
{ k = k + n;
uint16_t n, m, index, index1, index2; plm = f1[k] * x * pm1 - f2[k] * pm2;
float k, z, schmidtQuasiNorm[NUMPCUP]; Pcup[k] = plm;
Pcup[0] = 1.0; dPcup[k] = (float)(n) * (pm1 - x * plm) / z;
dPcup[0] = 0.0; pm2 = pm1;
/*sin (geocentric latitude) - sin_phi */ pm1 = plm;
z = sqrt( ( 1.0 - x ) * ( 1.0 + x ) ) ; }
/* First, Compute the Gauss-normalized associated Legendre functions*/ pmm = PreSqr[2] * scalef;
for (n = 1; n <= nMax; n++) rescalem = 1.0 / scalef;
{ kstart = 0;
for (m=0;m<=n;m++)
{ for (m = 1; m <= nMax - 1; ++m) {
index = (n * (n + 1) / 2 + m); rescalem = rescalem * z;
if (n == m)
{ /* Calculate Pcup(m,m) */
index1 = ( n - 1 ) * n / 2 + m -1; kstart = kstart + m + 1;
Pcup [index] = z * Pcup[index1]; pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m];
dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1];
} dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z);
else if (n == 1 && m == 0) pm2 = pmm / PreSqr[2 * m + 1];
{ /* Calculate Pcup(m+1,m) */
index1 = ( n - 1 ) * n / 2 + m; k = kstart + m + 1;
Pcup[index] = x * Pcup[index1]; pm1 = x * PreSqr[2 * m + 1] * pm2;
dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; Pcup[k] = pm1 * rescalem;
} dPcup[k] =
else if (n > 1 && n != m) ((pm2 * rescalem) * PreSqr[2 * m + 1] -
{ x * (float)(m + 1) * Pcup[k]) / z;
index1 = ( n - 2 ) * ( n - 1 ) / 2 + m; /* Calculate Pcup(n,m) */
index2 = ( n - 1) * n / 2 + m; for (n = m + 2; n <= nMax; ++n) {
if (m > n - 2) k = k + n;
{ plm = x * f1[k] * pm1 - f2[k] * pm2;
Pcup[index] = x * Pcup[index2]; Pcup[k] = plm * rescalem;
dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; dPcup[k] =
} (PreSqr[n + m] * PreSqr[n - m] *
else (pm1 * rescalem) -
{ (float)(n) * x * Pcup[k]) / z;
k = (float)( ( ( n - 1 ) * ( n - 1 ) ) - ( m * m ) ) / ( float ) ( ( 2 * n - 1 ) * ( 2 * n - 3 ) ); pm2 = pm1;
Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; pm1 = plm;
dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; }
} }
}
} /* Calculate Pcup(nMax,nMax) */
} rescalem = rescalem * z;
/*Compute the ration between the Gauss-normalized associated Legendre kstart = kstart + m + 1;
functions and the Schmidt quasi-normalized version. This is equivalent to pmm = pmm / PreSqr[2 * nMax];
sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ Pcup[kstart] = pmm * rescalem;
dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z;
schmidtQuasiNorm[0] = 1.0;
for (n = 1; n <= nMax; n++) return TRUE;
{ } /* WMM_PcupHigh */
index = (n * (n + 1) / 2);
index1 = (n - 1) * n / 2 ; uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
/* for m = 0 */
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float) (2 * n - 1) / (float) n; /* This function evaluates all of the Schmidt-semi normalized associated Legendre
functions up to degree nMax.
for ( m = 1; m <= n; m++)
{ Calling Parameters:
index = (n * (n + 1) / 2 + m); INPUT
index1 = (n * (n + 1) / 2 + m - 1); nMax: Maximum spherical harmonic degree to compute.
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt( (float) ((n - m + 1) * (m == 1 ? 2 : 1)) / (float) (n + m)); x: cos(colatitude) or sin(latitude).
}
OUTPUT
} Pcup: A vector of all associated Legendgre polynomials evaluated at
x up to nMax.
/* Converts the Gauss-normalized associated Legendre dPcup: Derivative of Pcup(x) with respect to latitude
functions to the Schmidt quasi-normalized version using pre-computed
relation stored in the variable schmidtQuasiNorm */ Notes: Overflow may occur if nMax > 20 , especially for high-latitudes.
Use WMM_PcupHigh for large nMax.
for (n = 1; n <= nMax; n++)
{ Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov.
for (m=0;m<=n;m++)
{ Note: In geomagnetism, the derivatives of ALF are usually found with
index = (n * (n + 1) / 2 + m); respect to the colatitudes. Here the derivatives are found with respect
Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; to the latitude. The difference is a sign reversal for the derivative of
dPcup[index] = - dPcup[index] * schmidtQuasiNorm[index]; the Associated Legendre Functions.
/* The sign is changed since the new WMM routines use derivative with respect to latitude */
insted of co-latitude */ {
} uint16_t n, m, index, index1, index2;
} float k, z, schmidtQuasiNorm[NUMPCUP];
Pcup[0] = 1.0;
return TRUE; dPcup[0] = 0.0;
} /*WMM_PcupLow */ /*sin (geocentric latitude) - sin_phi */
z = sqrt((1.0 - x) * (1.0 + x));
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) /* First, Compute the Gauss-normalized associated Legendre functions */
/* Special calculation for the component By at Geographic poles. for (n = 1; n <= nMax; n++) {
Manoj Nair, June, 2009 manoj.c.nair@noaa.gov for (m = 0; m <= n; m++) {
INPUT: MagneticModel index = (n * (n + 1) / 2 + m);
SphVariables if (n == m) {
CoordSpherical index1 = (n - 1) * n / 2 + m - 1;
OUTPUT: MagneticResults Pcup[index] = z * Pcup[index1];
CALLS : none dPcup[index] =
See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report z * dPcup[index1] + x * Pcup[index1];
} else if (n == 1 && m == 0) {
*/ index1 = (n - 1) * n / 2 + m;
{ Pcup[index] = x * Pcup[index1];
uint16_t n, index; dPcup[index] =
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; x * dPcup[index1] - z * Pcup[index1];
} else if (n > 1 && n != m) {
PcupS[0] = 1; index1 = (n - 2) * (n - 1) / 2 + m;
schmidtQuasiNorm1 = 1.0; index2 = (n - 1) * n / 2 + m;
if (m > n - 2) {
MagneticResults->By = 0.0; Pcup[index] = x * Pcup[index2];
sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); dPcup[index] =
x * dPcup[index2] -
for (n = 1; n <= MagneticModel->nMax; n++) z * Pcup[index2];
{ } else {
k = (float)(((n - 1) * (n - 1)) -
/*Compute the ration between the Gauss-normalized associated Legendre (m * m)) / (float)((2 *
functions and the Schmidt quasi-normalized version. This is equivalent to n -
sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ 1)
*
index = (n * (n + 1) / 2 + 1); (2 *
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; n -
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt( (float) (n * 2) / (float) (n + 1)); 3));
schmidtQuasiNorm1 = schmidtQuasiNorm2; Pcup[index] =
if (n == 1) x * Pcup[index2] -
{ k * Pcup[index1];
PcupS[n] = PcupS[n-1]; dPcup[index] =
} x * dPcup[index2] -
else z * Pcup[index2] -
{ k * dPcup[index1];
k = (float)( ( (n - 1) * (n - 1) ) - 1) / ( float ) ( (2 * n - 1) * (2 * n - 3) ); }
PcupS[n] = sin_phi * PcupS[n-1] - k * PcupS[n-2]; }
} }
}
/* 1 nMax (n+2) n m m m /*Compute the ration between the Gauss-normalized associated Legendre
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) functions and the Schmidt quasi-normalized version. This is equivalent to
n=1 m=0 n n n */ sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
schmidtQuasiNorm[0] = 1.0;
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * for (n = 1; n <= nMax; n++) {
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] - index = (n * (n + 1) / 2);
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] ) index1 = (n - 1) * n / 2;
* PcupS[n] * schmidtQuasiNorm3; /* for m = 0 */
} schmidtQuasiNorm[index] =
schmidtQuasiNorm[index1] * (float)(2 * n -
return TRUE; 1) / (float)n;
}/*WMM_SummationSpecial */
for (m = 1; m <= n; m++) {
uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) index = (n * (n + 1) / 2 + m);
{ index1 = (n * (n + 1) / 2 + m - 1);
/*Special calculation for the secular variation summation at the poles. schmidtQuasiNorm[index] =
schmidtQuasiNorm[index1] *
sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) /
INPUT: MagneticModel (float)(n + m));
SphVariables }
CoordSpherical
OUTPUT: MagneticResults }
CALLS : none
/* Converts the Gauss-normalized associated Legendre
functions to the Schmidt quasi-normalized version using pre-computed
*/ relation stored in the variable schmidtQuasiNorm */
uint16_t n, index;
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; for (n = 1; n <= nMax; n++) {
for (m = 0; m <= n; m++) {
PcupS[0] = 1; index = (n * (n + 1) / 2 + m);
schmidtQuasiNorm1 = 1.0; Pcup[index] =
Pcup[index] * schmidtQuasiNorm[index];
MagneticResults->By = 0.0; dPcup[index] =
sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); -dPcup[index] * schmidtQuasiNorm[index];
/* The sign is changed since the new WMM routines use derivative with respect to latitude
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) insted of co-latitude */
{ }
index = (n * (n + 1) / 2 + 1); }
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n;
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt( (float) (n * 2) / (float) (n + 1)); return TRUE;
schmidtQuasiNorm1 = schmidtQuasiNorm2; } /*WMM_PcupLow */
if (n == 1)
{ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
PcupS[n] = PcupS[n-1]; SphVariables,
} WMMtype_CoordSpherical * CoordSpherical,
else WMMtype_MagneticResults * MagneticResults)
{ /* Special calculation for the component By at Geographic poles.
k = (float)( ( (n - 1) * (n - 1) ) - 1) / ( float ) ( (2 * n - 1) * (2 * n - 3) ); Manoj Nair, June, 2009 manoj.c.nair@noaa.gov
PcupS[n] = sin_phi * PcupS[n-1] - k * PcupS[n-2]; INPUT: MagneticModel
} SphVariables
CoordSpherical
/* 1 nMax (n+2) n m m m OUTPUT: MagneticResults
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) CALLS : none
n=1 m=0 n n n */ See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report
/* Derivative with respect to longitude, divided by radius. */
*/
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * {
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] - uint16_t n, index;
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] ) float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1,
* PcupS[n] * schmidtQuasiNorm3; schmidtQuasiNorm2, schmidtQuasiNorm3;
}
PcupS[0] = 1;
return TRUE; schmidtQuasiNorm1 = 1.0;
}/*SecVarSummationSpecial*/
MagneticResults->By = 0.0;
sin_phi = sin(DEG2RAD(CoordSpherical->phig));
void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate)
// Time change the Model coefficients from the base year of the model using secular variation coefficients. for (n = 1; n <= MagneticModel->nMax; n++) {
//
// Modified to work on the global data structure to reduce memory footprint /*Compute the ration between the Gauss-normalized associated Legendre
{ functions and the Schmidt quasi-normalized version. This is equivalent to
uint16_t n, m, index, a, b; sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */
a = MagneticModel->nMaxSecVar; index = (n * (n + 1) / 2 + 1);
b = (a * (a + 1) / 2 + a); schmidtQuasiNorm2 =
for (n = 1; n <= MagneticModel->nMax; n++) schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
{ schmidtQuasiNorm3 =
for (m=0;m<=n;m++) schmidtQuasiNorm2 * sqrt((float)(n * 2) /
{ (float)(n + 1));
index = (n * (n + 1) / 2 + m); schmidtQuasiNorm1 = schmidtQuasiNorm2;
if(index <= b) if (n == 1) {
{ PcupS[n] = PcupS[n - 1];
MagneticModel->Main_Field_Coeff_H[index] += (UserDate->DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; } else {
MagneticModel->Main_Field_Coeff_G[index] += (UserDate->DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; k = (float)(((n - 1) * (n - 1)) -
} 1) / (float)((2 * n - 1) * (2 * n -
} 3));
} PcupS[n] =
} /* WMM_TimelyModifyMagneticModel */ sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
}
uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error)
// Converts a given calendar date into a decimal year /* 1 nMax (n+2) n m m m
{ By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
uint16_t temp = 0; // Total number of days n=1 m=0 n n n */
uint16_t MonthDays[13] = {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
uint16_t ExtraDay = 0;
uint16_t i; MagneticResults->By +=
SphVariables->RelativeRadiusPower[n] *
if((CalendarDate->Year%4 == 0 && CalendarDate->Year%100 != 0) || CalendarDate->Year%400 == 0) (MagneticModel->Main_Field_Coeff_G[index] *
ExtraDay=1; SphVariables->sin_mlambda[1] -
MonthDays[2] += ExtraDay; MagneticModel->Main_Field_Coeff_H[index] *
SphVariables->cos_mlambda[1])
/******************Validation********************************/ * PcupS[n] * schmidtQuasiNorm3;
if(CalendarDate->Month <= 0 || CalendarDate->Month > 12) }
{
strcpy(Error, "\nError: The Month entered is invalid, valid months are '1 to 12'\n"); return TRUE;
return 0; } /*WMM_SummationSpecial */
}
if(CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
{ SphVariables,
// printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]); WMMtype_CoordSpherical *
strcpy(Error, "\nError: The day entered is invalid\n"); CoordSpherical,
return 0; WMMtype_MagneticResults *
} MagneticResults)
/****************Calculation of t***************************/ {
for(i = 1; i <= CalendarDate->Month; i++) /*Special calculation for the secular variation summation at the poles.
temp+=MonthDays[i-1];
temp+=CalendarDate->Day; INPUT: MagneticModel
CalendarDate->DecimalYear = CalendarDate->Year + (temp-1)/(365.0 + ExtraDay); SphVariables
CoordSpherical
return 1; OUTPUT: MagneticResults
} /*WMM_DateToYear*/ CALLS : none
void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) */
// Converts Geodetic coordinates to Spherical coordinates uint16_t n, index;
// Convert geodetic coordinates, (defined by the WGS-84 float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1,
// reference ellipsoid), to Earth Centered Earth Fixed Cartesian schmidtQuasiNorm2, schmidtQuasiNorm3;
// coordinates, and then to spherical coordinates.
{ PcupS[0] = 1;
float CosLat, SinLat, rc, xp, zp; // all local variables schmidtQuasiNorm1 = 1.0;
CosLat = cos(DEG2RAD(CoordGeodetic->phi)); MagneticResults->By = 0.0;
SinLat = sin(DEG2RAD(CoordGeodetic->phi)); sin_phi = sin(DEG2RAD(CoordSpherical->phig));
// compute the local radius of curvature on the WGS-84 reference ellipsoid for (n = 1; n <= MagneticModel->nMaxSecVar; n++) {
rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat); index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 =
// compute ECEF Cartesian coordinates of specified point (for longitude=0) schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 =
xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; schmidtQuasiNorm2 * sqrt((float)(n * 2) /
zp = (rc*(1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2;
// compute spherical radius and angle lambda and phi of specified point if (n == 1) {
PcupS[n] = PcupS[n - 1];
CoordSpherical->r = sqrt(xp * xp + zp * zp); } else {
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude k = (float)(((n - 1) * (n - 1)) -
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude 1) / (float)((2 * n - 1) * (2 * n -
3));
}// WMM_GeodeticToSpherical PcupS[n] =
sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
void WMM_Set_Coeff_Array() }
{
// const should hopefully keep them in the flash region /* 1 nMax (n+2) n m m m
static const float CoeffFile[91][6] = By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
{{0, 0, 0, 0, 0, 0}, n=1 m=0 n n n */
{1, 0, -29496.6, 0.0, 11.6, 0.0}, /* Derivative with respect to longitude, divided by radius. */
{1, 1, -1586.3, 4944.4, 16.5, -25.9},
{2, 0, -2396.6, 0.0, -12.1, 0.0}, MagneticResults->By +=
{2, 1, 3026.1, -2707.7, -4.4, -22.5}, SphVariables->RelativeRadiusPower[n] *
{2, 2, 1668.6, -576.1, 1.9, -11.8}, (MagneticModel->Secular_Var_Coeff_G[index] *
{3, 0, 1340.1, 0.0, 0.4, 0.0}, SphVariables->sin_mlambda[1] -
{3, 1, -2326.2, -160.2, -4.1, 7.3}, MagneticModel->Secular_Var_Coeff_H[index] *
{3, 2, 1231.9, 251.9, -2.9, -3.9}, SphVariables->cos_mlambda[1])
{3, 3, 634.0, -536.6, -7.7, -2.6}, * PcupS[n] * schmidtQuasiNorm3;
{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}, return TRUE;
{4, 3, -357.1, 164.3, 4.6, 3.9}, } /*SecVarSummationSpecial */
{4, 4, 89.4, -309.1, -2.1, -0.8},
{5, 0, -230.9, 0.0, -1.0, 0.0}, void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate)
{5, 1, 357.2, 44.6, 0.6, 0.4}, // Time change the Model coefficients from the base year of the model using secular variation coefficients.
{5, 2, 200.3, 188.9, -1.8, 1.8}, //
{5, 3, -141.1, -118.2, -1.0, 1.2}, // Modified to work on the global data structure to reduce memory footprint
{5, 4, -163.0, 0.0, 0.9, 4.0}, {
{5, 5, -7.8, 100.9, 1.0, -0.6}, uint16_t n, m, index, a, b;
{6, 0, 72.8, 0.0, -0.2, 0.0},
{6, 1, 68.6, -20.8, -0.2, -0.2}, a = MagneticModel->nMaxSecVar;
{6, 2, 76.0, 44.1, -0.1, -2.1}, b = (a * (a + 1) / 2 + a);
{6, 3, -141.4, 61.5, 2.0, -0.4}, for (n = 1; n <= MagneticModel->nMax; n++) {
{6, 4, -22.8, -66.3, -1.7, -0.6}, for (m = 0; m <= n; m++) {
{6, 5, 13.2, 3.1, -0.3, 0.5}, index = (n * (n + 1) / 2 + m);
{6, 6, -77.9, 55.0, 1.7, 0.9}, if (index <= b) {
{7, 0, 80.5, 0.0, 0.1, 0.0}, MagneticModel->Main_Field_Coeff_H[index] +=
{7, 1, -75.1, -57.9, -0.1, 0.7}, (UserDate->DecimalYear -
{7, 2, -4.7, -21.1, -0.6, 0.3}, MagneticModel->epoch) *
{7, 3, 45.3, 6.5, 1.3, -0.1}, MagneticModel->
{7, 4, 13.9, 24.9, 0.4, -0.1}, Secular_Var_Coeff_H[index];
{7, 5, 10.4, 7.0, 0.3, -0.8}, MagneticModel->Main_Field_Coeff_G[index] +=
{7, 6, 1.7, -27.7, -0.7, -0.3}, (UserDate->DecimalYear -
{7, 7, 4.9, -3.3, 0.6, 0.3}, MagneticModel->epoch) *
{8, 0, 24.4, 0.0, -0.1, 0.0}, MagneticModel->
{8, 1, 8.1, 11.0, 0.1, -0.1}, Secular_Var_Coeff_G[index];
{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}, } /* WMM_TimelyModifyMagneticModel */
{8, 6, 10.9, 7.0, 0.3, -0.1},
{8, 7, -14.1, -10.8, -0.6, 0.4}, uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error)
{8, 8, -3.7, 1.7, 0.2, 0.3}, // Converts a given calendar date into a decimal year
{9, 0, 5.4, 0.0, 0.0, 0.0}, {
{9, 1, 9.4, -20.5, -0.1, 0.0}, uint16_t temp = 0; // Total number of days
{9, 2, 3.4, 11.5, 0.0, -0.2}, uint16_t MonthDays[13] =
{9, 3, -5.2, 12.8, 0.3, 0.0}, { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
{9, 4, 3.1, -7.2, -0.4, -0.1}, uint16_t ExtraDay = 0;
{9, 5, -12.4, -7.4, -0.3, 0.1}, uint16_t i;
{9, 6, -0.7, 8.0, 0.1, 0.0},
{9, 7, 8.4, 2.1, -0.1, -0.2}, if ((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0)
{9, 8, -8.5, -6.1, -0.4, 0.3}, || CalendarDate->Year % 400 == 0)
{9, 9, -10.1, 7.0, -0.2, 0.2}, ExtraDay = 1;
{10, 0, -2.0, 0.0, 0.0, 0.0}, MonthDays[2] += ExtraDay;
{10, 1, -6.3, 2.8, 0.0, 0.1},
{10, 2, 0.9, -0.1, -0.1, -0.1}, /******************Validation********************************/
{10, 3, -1.1, 4.7, 0.2, 0.0}, if (CalendarDate->Month <= 0 || CalendarDate->Month > 12) {
{10, 4, -0.2, 4.4, 0.0, -0.1}, strcpy(Error,
{10, 5, 2.5, -7.2, -0.1, -0.1}, "\nError: The Month entered is invalid, valid months are '1 to 12'\n");
{10, 6, -0.3, -1.0, -0.2, 0.0}, return 0;
{10, 7, 2.2, -3.9, 0.0, -0.1}, }
{10, 8, 3.1, -2.0, -0.1, -0.2}, if (CalendarDate->Day <= 0
{10, 9, -1.0, -2.0, -0.2, 0.0}, || CalendarDate->Day > MonthDays[CalendarDate->Month]) {
{10, 10, -2.8, -8.3, -0.2, -0.1}, // printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]);
{11, 0, 3.0, 0.0, 0.0, 0.0}, strcpy(Error, "\nError: The day entered is invalid\n");
{11, 1, -1.5, 0.2, 0.0, 0.0}, return 0;
{11, 2, -2.1, 1.7, 0.0, 0.1}, }
{11, 3, 1.7, -0.6, 0.1, 0.0}, /****************Calculation of t***************************/
{11, 4, -0.5, -1.8, 0.0, 0.1}, for (i = 1; i <= CalendarDate->Month; i++)
{11, 5, 0.5, 0.9, 0.0, 0.0}, temp += MonthDays[i - 1];
{11, 6, -0.8, -0.4, 0.0, 0.1}, temp += CalendarDate->Day;
{11, 7, 0.4, -2.5, 0.0, 0.0}, CalendarDate->DecimalYear =
{11, 8, 1.8, -1.3, 0.0, -0.1}, CalendarDate->Year + (temp - 1) / (365.0 + ExtraDay);
{11, 9, 0.1, -2.1, 0.0, -0.1},
{11, 10, 0.7, -1.9, -0.1, 0.0}, return 1;
{11, 11, 3.8, -1.8, 0.0, -0.1}, } /*WMM_DateToYear */
{12, 0, -2.2, 0.0, 0.0, 0.0},
{12, 1, -0.2, -0.9, 0.0, 0.0}, void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic,
{12, 2, 0.3, 0.3, 0.1, 0.0}, WMMtype_CoordSpherical * CoordSpherical)
{12, 3, 1.0, 2.1, 0.1, 0.0}, // Converts Geodetic coordinates to Spherical coordinates
{12, 4, -0.6, -2.5, -0.1, 0.0}, // Convert geodetic coordinates, (defined by the WGS-84
{12, 5, 0.9, 0.5, 0.0, 0.0}, // reference ellipsoid), to Earth Centered Earth Fixed Cartesian
{12, 6, -0.1, 0.6, 0.0, 0.1}, // coordinates, and then to spherical coordinates.
{12, 7, 0.5, 0.0, 0.0, 0.0}, {
{12, 8, -0.4, 0.1, 0.0, 0.0}, float CosLat, SinLat, rc, xp, zp; // all local variables
{12, 9, -0.4, 0.3, 0.0, 0.0},
{12, 10, 0.2, -0.9, 0.0, 0.0}, CosLat = cos(DEG2RAD(CoordGeodetic->phi));
{12, 11, -0.8, -0.2, -0.1, 0.0}, SinLat = sin(DEG2RAD(CoordGeodetic->phi));
{12, 12, 0.0, 0.9, 0.1, 0.0}};
// compute the local radius of curvature on the WGS-84 reference ellipsoid
// TODO: If this works here, delete first two columns to save space rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat);
for(uint16_t i=0; i<NUMTERMS; i++){
MagneticModel->Main_Field_Coeff_G[i]=CoeffFile[i][2]; // compute ECEF Cartesian coordinates of specified point (for longitude=0)
MagneticModel->Main_Field_Coeff_H[i]=CoeffFile[i][3];
MagneticModel->Secular_Var_Coeff_G[i]=CoeffFile[i][4]; xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
MagneticModel->Secular_Var_Coeff_H[i]=CoeffFile[i][5]; zp = (rc * (1.0 - Ellip->epssq) +
} CoordGeodetic->HeightAboveEllipsoid) * SinLat;
} // compute spherical radius and angle lambda and phi of specified point
CoordSpherical->r = sqrt(xp * xp + zp * zp);
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
} // WMM_GeodeticToSpherical
void WMM_Set_Coeff_Array()
{
// const should hopefully keep them in the flash region
static const float 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}
};
// TODO: If this works here, delete first two columns to save space
for (uint16_t i = 0; i < NUMTERMS; i++) {
MagneticModel->Main_Field_Coeff_G[i] = CoeffFile[i][2];
MagneticModel->Main_Field_Coeff_H[i] = CoeffFile[i][3];
MagneticModel->Secular_Var_Coeff_G[i] = CoeffFile[i][4];
MagneticModel->Secular_Var_Coeff_H[i] = CoeffFile[i][5];
}
}

482
flight/Libraries/buffer.c
View File

@ -1,244 +1,238 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @addtogroup OpenPilotModules OpenPilot Modules * @addtogroup OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup Flight_Libraries Miscellaneous library functions * @addtogroup Flight_Libraries Miscellaneous library functions
* @brief Miscellaneous library functions shared between PIOS / OpenPilot / AHRS * @brief Miscellaneous library functions shared between PIOS / OpenPilot / AHRS
* @{ * @{
* *
* @file buffer.c * @file buffer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Simplies buffering data * @brief Simplies buffering data
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
//***************************************************************************** //*****************************************************************************
// //
// File Name : 'buffer.c' // File Name : 'buffer.c'
// Title : Multipurpose byte buffer structure and methods // Title : Multipurpose byte buffer structure and methods
// Author : Pascal Stang - Copyright (C) 2001-2002 // Author : Pascal Stang - Copyright (C) 2001-2002
// Created : 9/23/2001 // Created : 9/23/2001
// Revised : 9/23/2001 // Revised : 9/23/2001
// Version : 1.0 // Version : 1.0
// Target MCU : any // Target MCU : any
// Editor Tabs : 4 // Editor Tabs : 4
// //
// This code is distributed under the GNU Public License // This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt // which can be found at http://www.gnu.org/licenses/gpl.txt
// //
//***************************************************************************** //*****************************************************************************
#include "buffer.h" #include "buffer.h"
/** /**
* @brief Initialize a cBuffer structure * @brief Initialize a cBuffer structure
* @param[in] buffer Points to the buffer structure * @param[in] buffer Points to the buffer structure
* @param[in] start Allocated memory to store data * @param[in] start Allocated memory to store data
* @param[in] size Maximum size of buffer * @param[in] size Maximum size of buffer
* @return None * @return None
*/ */
void bufferInit(cBuffer* buffer, uint8_t *start, uint32_t size) void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size)
{ {
// set start pointer of the buffer // set start pointer of the buffer
buffer->dataptr = start; buffer->dataptr = start;
buffer->size = size; buffer->size = size;
// initialize index and length // initialize index and length
buffer->dataindex = 0; buffer->dataindex = 0;
buffer->datalength = 0; buffer->datalength = 0;
} }
/** /**
* @brief Return remaining space in buffer * @brief Return remaining space in buffer
* @param[in] buffer Pointer to buffer structure * @param[in] buffer Pointer to buffer structure
* @return Amount of space remaining on buffer * @return Amount of space remaining on buffer
*/ */
uint32_t bufferRemainingSpace(cBuffer* buffer) uint32_t bufferRemainingSpace(cBuffer * buffer)
{ {
return buffer->size - buffer->datalength; return buffer->size - buffer->datalength;
} }
/** /**
* @brief Return amount of data * @brief Return amount of data
* @param[in] buffer Pointer to buffer structure * @param[in] buffer Pointer to buffer structure
* @return Amount of data queued in buffer * @return Amount of data queued in buffer
*/ */
uint32_t bufferBufferedData(cBuffer* buffer) uint32_t bufferBufferedData(cBuffer * buffer)
{ {
return buffer->datalength; return buffer->datalength;
} }
/**
/** * @brief Pop one element from buffer
* @brief Pop one element from buffer * @param[in] buffer Pointer to the buffer structure
* @param[in] buffer Pointer to the buffer structure * @return None
* @return None */
*/ uint8_t bufferGetFromFront(cBuffer * buffer)
uint8_t bufferGetFromFront(cBuffer* buffer) {
{ unsigned char data = 0;
unsigned char data = 0;
// check to see if there's data in the buffer
// check to see if there's data in the buffer if (buffer->datalength) {
if(buffer->datalength) // get the first character from buffer
{ data = buffer->dataptr[buffer->dataindex];
// get the first character from buffer // move index down and decrement length
data = buffer->dataptr[buffer->dataindex]; buffer->dataindex++;
// move index down and decrement length if (buffer->dataindex >= buffer->size) {
buffer->dataindex++; buffer->dataindex %= buffer->size;
if(buffer->dataindex >= buffer->size) }
{ buffer->datalength--;
buffer->dataindex %= buffer->size; }
} // return
buffer->datalength--; return data;
} }
// return
return data; /**
} * @brief Copy number of elements into another buffer
* @param[in] buffer Pointer to the buffer structure
/** * @param[in] dest Point to destimation, must be allocated enough space for size
* @brief Copy number of elements into another buffer * @param[in] size Number of elements to get
* @param[in] buffer Pointer to the buffer structure * @return
* @param[in] dest Point to destimation, must be allocated enough space for size * @arg -1 for success
* @param[in] size Number of elements to get * @arg 0 error
* @return */
* @arg -1 for success uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest,
* @arg 0 error uint32_t size)
*/ {
uint8_t bufferGetChunkFromFront(cBuffer* buffer, uint8_t * dest, uint32_t size) if (size > buffer->datalength)
{ return -1;
if(size > buffer->datalength)
return -1; for (uint32_t i = 0; i < size; i++) {
dest[i] = bufferGetFromFront(buffer);
for(uint32_t i = 0; i < size; i++) }
{
dest[i] = bufferGetFromFront(buffer); return 0;
} }
return 0; /**
} * @brief Shift index to trash data from front of buffer
* @param[in] buffer Pointer to buffer structure
/** * @param[in] numbytes Number of bytes to drop
* @brief Shift index to trash data from front of buffer * @return None
* @param[in] buffer Pointer to buffer structure */
* @param[in] numbytes Number of bytes to drop void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes)
* @return None {
*/ // dump numbytes from the front of the buffer
void bufferDumpFromFront(cBuffer* buffer, uint32_t numbytes) // are we dumping less than the entire buffer?
{ if (numbytes < buffer->datalength) {
// dump numbytes from the front of the buffer // move index down by numbytes and decrement length by numbytes
// are we dumping less than the entire buffer? buffer->dataindex += numbytes;
if(numbytes < buffer->datalength) if (buffer->dataindex >= buffer->size) {
{ buffer->dataindex %= buffer->size;
// move index down by numbytes and decrement length by numbytes }
buffer->dataindex += numbytes; buffer->datalength -= numbytes;
if(buffer->dataindex >= buffer->size) } else {
{ // flush the whole buffer
buffer->dataindex %= buffer->size; buffer->datalength = 0;
} }
buffer->datalength -= numbytes; }
}
else /**
{ * @brief Get element indexed from the front of buffer
// flush the whole buffer * @param[in] buffer Point to the buffer structure
buffer->datalength = 0; * @param[in] index Index into the buffer relative to front
} * @return None
} */
uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index)
/** {
* @brief Get element indexed from the front of buffer // return character at index in buffer
* @param[in] buffer Point to the buffer structure return buffer->dataptr[(buffer->dataindex + index) %
* @param[in] index Index into the buffer relative to front (buffer->size)];
* @return None }
*/
uint8_t bufferGetAtIndex(cBuffer* buffer, uint32_t index) /**
{ * @brief Queue a character to end of buffer
// return character at index in buffer * @param[in] buffer Point to the buffer structure
return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)]; * @param[in] data Byte to add
} * @return
* @arg -1 for success
/** * @arg 0 error
* @brief Queue a character to end of buffer */
* @param[in] buffer Point to the buffer structure uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data)
* @param[in] data Byte to add {
* @return // make sure the buffer has room
* @arg -1 for success if (buffer->datalength < buffer->size) {
* @arg 0 error // save data byte at end of buffer
*/ buffer->dataptr[(buffer->dataindex + buffer->datalength) %
uint8_t bufferAddToEnd(cBuffer* buffer, uint8_t data) buffer->size] = data;
{ // increment the length
// make sure the buffer has room buffer->datalength++;
if(buffer->datalength < buffer->size) // return success
{ return -1;
// save data byte at end of buffer } else
buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; return 0;
// increment the length }
buffer->datalength++;
// return success /**
return -1; * @brief Queue a block of character to end of buffer
} * @param[in] buffer Point to the buffer structure
else return 0; * @param[in] data Pointer to data to add
} * @param[in] size Number of bytes to add
* @return
/** * @arg -1 for success
* @brief Queue a block of character to end of buffer * @arg 0 error
* @param[in] buffer Point to the buffer structure */
* @param[in] data Pointer to data to add uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data,
* @param[in] size Number of bytes to add uint32_t size)
* @return {
* @arg -1 for success // TODO: replace with memcpy and logic, for now keeping it simple
* @arg 0 error for (uint32_t i = 0; i < size; i++) {
*/ if (bufferAddToEnd(buffer, data[i]) == 0)
uint8_t bufferAddChunkToEnd(cBuffer* buffer, const uint8_t * data, uint32_t size) return 0;
{ }
// TODO: replace with memcpy and logic, for now keeping it simple return -1;
for(uint32_t i = 0; i < size; i++) }
{
if(bufferAddToEnd(buffer,data[i]) == 0) /**
return 0; * @brief Check to see if the buffer has room
} * @param[in] buffer Point to the buffer structure
return -1; * @return
} * @arg True there is room available in buffer
* @arg False buffer is full
/** */
* @brief Check to see if the buffer has room unsigned char bufferIsNotFull(cBuffer * buffer)
* @param[in] buffer Point to the buffer structure {
* @return return (buffer->datalength < buffer->size);
* @arg True there is room available in buffer }
* @arg False buffer is full
*/ /**
unsigned char bufferIsNotFull(cBuffer* buffer) * @brief Trash all data in buffer
{ * @param[in] buffer Point to the buffer structure
return (buffer->datalength < buffer->size); */
} void bufferFlush(cBuffer * buffer)
{
/** // flush contents of the buffer
* @brief Trash all data in buffer buffer->datalength = 0;
* @param[in] buffer Point to the buffer structure }
*/
void bufferFlush(cBuffer* buffer) /**
{ * @}
// flush contents of the buffer * @}
buffer->datalength = 0; */
}
/**
* @}
* @}
*/

114
flight/Libraries/inc/CoordinateConversions.h
View File

@ -1,56 +1,58 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file CoordinateConverions.h * @file CoordinateConverions.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Header for Coordinate conversions library in CoordinateConversions.c * @brief Header for Coordinate conversions library in CoordinateConversions.c
* - all angles in deg * - all angles in deg
* - distances in meters * - distances in meters
* - altitude above WGS-84 elipsoid * - altitude above WGS-84 elipsoid
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef COORDINATECONVERSIONS_H_ #ifndef COORDINATECONVERSIONS_H_
#define COORDINATECONVERSIONS_H_ #define COORDINATECONVERSIONS_H_
// ****** convert Lat,Lon,Alt to ECEF ************ // ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(double LLA[3], double ECEF[3]); void LLA2ECEF(double LLA[3], double ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(double ECEF[3], double LLA[3]); uint16_t ECEF2LLA(double ECEF[3], double LLA[3]);
void RneFromLLA(double LLA[3], float Rne[3][3]); void RneFromLLA(double LLA[3], float Rne[3][3]);
// ****** find roll, pitch, yaw from quaternion ******** // ****** find roll, pitch, yaw from quaternion ********
void Quaternion2RPY(float q[4], float rpy[3]); void Quaternion2RPY(float q[4], float rpy[3]);
// ****** find quaternion from roll, pitch, yaw ******** // ****** find quaternion from roll, pitch, yaw ********
void RPY2Quaternion(float rpy[3], float q[4]); void RPY2Quaternion(float rpy[3], float q[4]);
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]); void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ******** // ****** Express LLA in a local NED Base Frame ********
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]); void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3],
float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]); // ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3],
#endif // COORDINATECONVERSIONS_H_ float NED[3]);
#endif // COORDINATECONVERSIONS_H_

353
flight/Libraries/inc/WMMInternal.h
View File

@ -1,168 +1,185 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file WMMInternal.h * @file WMMInternal.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the WorldMagModel internal functionality. * @brief Include file of the WorldMagModel internal functionality.
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef WMMINTERNAL_H_ #ifndef WMMINTERNAL_H_
#define WMMINTERNAL_H_ #define WMMINTERNAL_H_
// internal constants // internal constants
#define TRUE ((uint16_t)1) #define TRUE ((uint16_t)1)
#define FALSE ((uint16_t)0) #define FALSE ((uint16_t)0)
#define WMM_MAX_MODEL_DEGREES 12 #define WMM_MAX_MODEL_DEGREES 12
#define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12 #define WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES 12
#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); #define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2);
#define NUMPCUP 92 // NUMTERMS +1 #define NUMPCUP 92 // NUMTERMS +1
#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 #define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1
#define RAD2DEG(rad) ((rad)*(180.0L/M_PI)) #define RAD2DEG(rad) ((rad)*(180.0L/M_PI))
#define DEG2RAD(deg) ((deg)*(M_PI/180.0L)) #define DEG2RAD(deg) ((deg)*(M_PI/180.0L))
// internal structure definitions
// internal structure definitions typedef struct {
typedef struct { float EditionDate;
float EditionDate; float epoch; //Base time of Geomagnetic model epoch (yrs)
float epoch; //Base time of Geomagnetic model epoch (yrs) char ModelName[20];
char ModelName[20]; float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT)
float Main_Field_Coeff_G[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT)
float Main_Field_Coeff_H[NUMTERMS]; // C - Gauss coefficients of main geomagnetic model (nT) float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr)
float Secular_Var_Coeff_G[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr)
float Secular_Var_Coeff_H[NUMTERMS]; // CD - Gauss coefficients of secular geomagnetic model (nT/yr) uint16_t nMax; // Maximum degree of spherical harmonic model
uint16_t nMax; // Maximum degree of spherical harmonic model uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model
uint16_t nMaxSecVar; // Maxumum degree of spherical harmonic secular model uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program
uint16_t SecularVariationUsed; // Whether or not the magnetic secular variation vector will be needed by program } WMMtype_MagneticModel;
} WMMtype_MagneticModel;
typedef struct {
typedef struct { float a; // semi-major axis of the ellipsoid
float a; // semi-major axis of the ellipsoid float b; // semi-minor axis of the ellipsoid
float b; // semi-minor axis of the ellipsoid float fla; // flattening
float fla; // flattening float epssq; // first eccentricity squared
float epssq; // first eccentricity squared float eps; // first eccentricity
float eps; // first eccentricity float re; // mean radius of ellipsoid
float re; // mean radius of ellipsoid } WMMtype_Ellipsoid;
} WMMtype_Ellipsoid;
typedef struct {
typedef struct { float lambda; // longitude
float lambda; // longitude float phi; // geodetic latitude
float phi; // geodetic latitude float HeightAboveEllipsoid; // height above the ellipsoid (HaE)
float HeightAboveEllipsoid; // height above the ellipsoid (HaE) } WMMtype_CoordGeodetic;
} WMMtype_CoordGeodetic;
typedef struct {
typedef struct { float lambda; // longitude
float lambda; // longitude float phig; // geocentric latitude
float phig; // geocentric latitude float r; // distance from the center of the ellipsoid
float r; // distance from the center of the ellipsoid } WMMtype_CoordSpherical;
} WMMtype_CoordSpherical;
typedef struct {
typedef struct { uint16_t Year;
uint16_t Year; uint16_t Month;
uint16_t Month; uint16_t Day;
uint16_t Day; float DecimalYear;
float DecimalYear; } WMMtype_Date;
} WMMtype_Date;
typedef struct {
typedef struct { float Pcup[NUMPCUP]; // Legendre Function
float Pcup[NUMPCUP]; // Legendre Function float dPcup[NUMPCUP]; // Derivative of Lagendre fn
float dPcup[NUMPCUP]; // Derivative of Lagendre fn } WMMtype_LegendreFunction;
} WMMtype_LegendreFunction;
typedef struct {
typedef struct { float Bx; // North
float Bx; // North float By; // East
float By; // East float Bz; // Down
float Bz; // Down } WMMtype_MagneticResults;
} WMMtype_MagneticResults;
typedef struct {
typedef struct {
float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES + 1]; // [earth_reference_radius_km / sph. radius ]^n
float RelativeRadiusPower[WMM_MAX_MODEL_DEGREES+1]; // [earth_reference_radius_km / sph. radius ]^n float cos_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // cp(m) - cosine of (m*spherical coord. longitude
float cos_mlambda[WMM_MAX_MODEL_DEGREES+1]; // cp(m) - cosine of (m*spherical coord. longitude float sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude)
float sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; // sp(m) - sine of (m*spherical coord. longitude) } WMMtype_SphericalHarmonicVariables;
} WMMtype_SphericalHarmonicVariables;
typedef struct {
typedef struct { float Decl; /* 1. Angle between the magnetic field vector and true north, positive east */
float Decl; /* 1. Angle between the magnetic field vector and true north, positive east*/ float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down */
float Incl; /*2. Angle between the magnetic field vector and the horizontal plane, positive down*/ float F; /*3. Magnetic Field Strength */
float F; /*3. Magnetic Field Strength*/ float H; /*4. Horizontal Magnetic Field Strength */
float H; /*4. Horizontal Magnetic Field Strength*/ float X; /*5. Northern component of the magnetic field vector */
float X; /*5. Northern component of the magnetic field vector*/ float Y; /*6. Eastern component of the magnetic field vector */
float Y; /*6. Eastern component of the magnetic field vector*/ float Z; /*7. Downward component of the magnetic field vector */
float Z; /*7. Downward component of the magnetic field vector*/ float GV; /*8. The Grid Variation */
float GV; /*8. The Grid Variation*/ float Decldot; /*9. Yearly Rate of change in declination */
float Decldot; /*9. Yearly Rate of change in declination*/ float Incldot; /*10. Yearly Rate of change in inclination */
float Incldot; /*10. Yearly Rate of change in inclination*/ float Fdot; /*11. Yearly rate of change in Magnetic field strength */
float Fdot; /*11. Yearly rate of change in Magnetic field strength*/ float Hdot; /*12. Yearly rate of change in horizontal field strength */
float Hdot; /*12. Yearly rate of change in horizontal field strength*/ float Xdot; /*13. Yearly rate of change in the northern component */
float Xdot; /*13. Yearly rate of change in the northern component*/ float Ydot; /*14. Yearly rate of change in the eastern component */
float Ydot; /*14. Yearly rate of change in the eastern component*/ float Zdot; /*15. Yearly rate of change in the downward component */
float Zdot; /*15. Yearly rate of change in the downward component*/ float GVdot; /*16. Yearly rate of chnage in grid variation */
float GVdot; /*16. Yearly rate of chnage in grid variation*/ } WMMtype_GeoMagneticElements;
} WMMtype_GeoMagneticElements;
// Internal Function Prototypes
// Internal Function Prototypes void WMM_Set_Coeff_Array();
void WMM_Set_Coeff_Array(); void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic,
void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); WMMtype_CoordSpherical * CoordSpherical);
uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error); uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error);
void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate); void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate);
uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordGeodetic * CoordGeodetic,
WMMtype_GeoMagneticElements *GeoMagneticElements); WMMtype_GeoMagneticElements * GeoMagneticElements);
uint16_t WMM_AssociatedLegendreFunction( WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction); uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *
CoordSpherical, uint16_t nMax,
uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); WMMtype_LegendreFunction *
LegendreFunction);
uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements);
uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *
uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_CoordSpherical *CoordSpherical, MagneticResultsGeo,
uint16_t nMax, WMMtype_GeoMagneticElements *
WMMtype_SphericalHarmonicVariables * SphVariables); GeoMagneticElements);
uint16_t WMM_PcupLow( float *Pcup, float *dPcup, float x, uint16_t nMax); uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *
MagneticVariation,
uint16_t WMM_PcupHigh( float *Pcup, float *dPcup, float x, uint16_t nMax); WMMtype_GeoMagneticElements *
MagneticElements);
uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * ,
WMMtype_CoordGeodetic * CoordGeodetic, uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *
WMMtype_MagneticResults * MagneticResultsSph, CoordSpherical,
WMMtype_MagneticResults *MagneticResultsGeo); uint16_t nMax,
WMMtype_SphericalHarmonicVariables
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, * SphVariables);
WMMtype_SphericalHarmonicVariables * SphVariables,
WMMtype_CoordSpherical * CoordSpherical, uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax);
WMMtype_MagneticResults *MagneticResults);
uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax);
uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
WMMtype_CoordSpherical * CoordSpherical, uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical *,
WMMtype_MagneticResults *MagneticResults); WMMtype_CoordGeodetic * CoordGeodetic,
WMMtype_MagneticResults *
uint16_t WMM_Summation( WMMtype_LegendreFunction *LegendreFunction, MagneticResultsSph,
WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_MagneticResults *
WMMtype_CoordSpherical * CoordSpherical, MagneticResultsGeo);
WMMtype_MagneticResults *MagneticResults);
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_SphericalHarmonicVariables *
WMMtype_CoordSpherical * CoordSpherical, SphVariables,
WMMtype_MagneticResults *MagneticResults); WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
#endif /* WMMINTERNAL_H_ */ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical *
CoordSpherical,
WMMtype_MagneticResults *
MagneticResults);
uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
WMMtype_SphericalHarmonicVariables * SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
#endif /* WMMINTERNAL_H_ */

70
flight/Libraries/inc/WorldMagModel.h
View File

@ -1,34 +1,36 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file WorldMagModel.h * @file WorldMagModel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the WorldMagModel exposed functionality. * @brief Include file of the WorldMagModel exposed functionality.
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * 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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef WORLDMAGMODEL_H_ #ifndef WORLDMAGMODEL_H_
#define WORLDMAGMODEL_H_ #define WORLDMAGMODEL_H_
// Exposed Function Prototypes // Exposed Function Prototypes
int WMM_Initialize(); int WMM_Initialize();
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]); void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid,
uint16_t Month, uint16_t Day, uint16_t Year,
#endif /* WORLDMAGMODEL_H_ */ float B[3]);
#endif /* WORLDMAGMODEL_H_ */

213
flight/Libraries/inc/buffer.h
View File

@ -1,106 +1,107 @@
/** /**
****************************************************************************** ******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules * @addtogroup OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup GSPModule GPS Module * @addtogroup GSPModule GPS Module
* @brief Process GPS information * @brief Process GPS information
* @{ * @{
* *
* @file buffer.c * @file buffer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief see below * @brief see below
* As with all modules only the initialize function is exposed all other * As with all modules only the initialize function is exposed all other
* interactions with the module take place through the event queue and * interactions with the module take place through the event queue and
* objects. * objects.
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
//***************************************************************************** //*****************************************************************************
// //
// File Name : 'buffer.h' // File Name : 'buffer.h'
// Title : Multipurpose byte buffer structure and methods // Title : Multipurpose byte buffer structure and methods
// Author : Pascal Stang - Copyright (C) 2001-2002 // Author : Pascal Stang - Copyright (C) 2001-2002
// Created : 9/23/2001 // Created : 9/23/2001
// Revised : 11/16/2002 // Revised : 11/16/2002
// Version : 1.1 // Version : 1.1
// Target MCU : any // Target MCU : any
// Editor Tabs : 4 // Editor Tabs : 4
// //
/// \code #include "buffer.h" \endcode /// \code #include "buffer.h" \endcode
/// \par Overview /// \par Overview
/// This byte-buffer structure provides an easy and efficient way to store /// This byte-buffer structure provides an easy and efficient way to store
/// and process a stream of bytes.  You can create as many buffers as you /// and process a stream of bytes.  You can create as many buffers as you
/// like (within memory limits), and then use this common set of functions to /// like (within memory limits), and then use this common set of functions to
/// access each buffer.  The buffers are designed for FIFO operation (first /// access each buffer.  The buffers are designed for FIFO operation (first
/// in, first out).  This means that the first byte you put in the buffer /// in, first out).  This means that the first byte you put in the buffer
/// will be the first one you get when you read out the buffer.  Supported /// will be the first one you get when you read out the buffer.  Supported
/// functions include buffer initialize, get byte from front of buffer, add /// functions include buffer initialize, get byte from front of buffer, add
/// byte to end of buffer, check if buffer is full, and flush buffer.  The /// byte to end of buffer, check if buffer is full, and flush buffer.  The
/// buffer uses a circular design so no copying of data is ever necessary. /// buffer uses a circular design so no copying of data is ever necessary.
/// This buffer is not dynamically allocated, it has a user-defined fixed /// This buffer is not dynamically allocated, it has a user-defined fixed
/// maximum size.  This buffer is used in many places in the avrlib code. /// maximum size.  This buffer is used in many places in the avrlib code.
// //
// This code is distributed under the GNU Public License // This code is distributed under the GNU Public License
// which can be found at http://www.gnu.org/licenses/gpl.txt // which can be found at http://www.gnu.org/licenses/gpl.txt
// //
//***************************************************************************** //*****************************************************************************
//@{ //@{
#ifndef BUFFER_H #ifndef BUFFER_H
#define BUFFER_H #define BUFFER_H
#include "stdint.h" #include "stdint.h"
// structure/typdefs // structure/typdefs
//! cBuffer structure //! cBuffer structure
typedef struct struct_cBuffer typedef struct struct_cBuffer {
{ unsigned char *dataptr; ///< the physical memory address where the buffer is stored
unsigned char *dataptr; ///< the physical memory address where the buffer is stored unsigned short size; ///< the allocated size of the buffer
unsigned short size; ///< the allocated size of the buffer unsigned short datalength; ///< the length of the data currently in the buffer
unsigned short datalength; ///< the length of the data currently in the buffer unsigned short dataindex; ///< the index into the buffer where the data starts
unsigned short dataindex; ///< the index into the buffer where the data starts } cBuffer;
} cBuffer;
// function prototypes
// function prototypes
//! initialize a buffer to start at a given address and have given size
//! initialize a buffer to start at a given address and have given size void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size);
void bufferInit(cBuffer* buffer, uint8_t *start, uint32_t size);
//! check free space in buffer
//! check free space in buffer uint32_t bufferRemainingSpace(cBuffer * buffer);
uint32_t bufferRemainingSpace(cBuffer* buffer);
//! get the first byte from the front of the buffer
//! get the first byte from the front of the buffer uint8_t bufferGetFromFront(cBuffer * buffer);
uint8_t bufferGetFromFront(cBuffer* buffer);
//! get the number of bytes buffered
//! get the number of bytes buffered uint32_t bufferBufferedData(cBuffer * buffer);
uint32_t bufferBufferedData(cBuffer* buffer);
//! copy number of elements into another buffer
//! copy number of elements into another buffer uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest,
uint8_t bufferGetChunkFromFront(cBuffer* buffer, uint8_t * dest, uint32_t size); uint32_t size);
//! dump (discard) the first numbytes from the front of the buffer //! dump (discard) the first numbytes from the front of the buffer
void bufferDumpFromFront(cBuffer* buffer, uint32_t numbytes); void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes);
//! get a byte at the specified index in the buffer (kind of like array access) //! get a byte at the specified index in the buffer (kind of like array access)
// ** note: this does not remove the byte that was read from the buffer // ** note: this does not remove the byte that was read from the buffer
uint8_t bufferGetAtIndex(cBuffer* buffer, uint32_t index); uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index);
//! add a byte to the end of the buffer //! add a byte to the end of the buffer
uint8_t bufferAddToEnd(cBuffer* buffer, uint8_t data); uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data);
//! queue a block of character to end of buffer //! queue a block of character to end of buffer
uint8_t bufferAddChunkToEnd(cBuffer* buffer, const uint8_t * data, uint32_t size); uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data,
uint32_t size);
//! check if the buffer is full/not full (returns non-zero value if not full)
uint8_t bufferIsNotFull(cBuffer* buffer); //! check if the buffer is full/not full (returns non-zero value if not full)
uint8_t bufferIsNotFull(cBuffer * buffer);
//! flush (clear) the contents of the buffer
void bufferFlush(cBuffer* buffer); //! flush (clear) the contents of the buffer
void bufferFlush(cBuffer * buffer);
#endif
#endif
/**
* @} /**
*/ * @}
*/