From 833e8428d211ba78e10137e06f0299caa46fa490 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Tue, 21 Sep 2010 19:29:35 +0000 Subject: [PATCH] Flight/Libraries: Updated to coding conventions 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 --- flight/Libraries/CoordinateConversions.c | 412 ++-- flight/Libraries/WorldMagModel.c | 2222 +++++++++--------- flight/Libraries/buffer.c | 482 ++-- flight/Libraries/inc/CoordinateConversions.h | 114 +- flight/Libraries/inc/WMMInternal.h | 353 +-- flight/Libraries/inc/WorldMagModel.h | 70 +- flight/Libraries/inc/buffer.h | 213 +- 7 files changed, 2016 insertions(+), 1850 deletions(-) diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c index b3a92ab28..cf3763457 100644 --- a/flight/Libraries/CoordinateConversions.c +++ b/flight/Libraries/CoordinateConversions.c @@ -1,187 +1,225 @@ -/** - ****************************************************************************** - * - * @file CoordinateConversions.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief General conversions with different coordinate systems. - * - all angles in deg - * - distances in meters - * - altitude above WGS-84 elipsoid - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include -#include -#include "CoordinateConversions.h" - -#define RAD2DEG (180.0/M_PI) -#define DEG2RAD (M_PI/180.0) - -// ****** convert Lat,Lon,Alt to ECEF ************ -void LLA2ECEF(double LLA[3], double ECEF[3]){ - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double sinLat, sinLon, cosLat, cosLon; - double N; - - sinLat=sin(DEG2RAD*LLA[0]); - sinLon=sin(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 - - ECEF[0] = (N+LLA[2])*cosLat*cosLon; - 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]) -{ - const double a = 6378137.0; // Equatorial Radius - const double e = 8.1819190842622e-2; // Eccentricity - double x=ECEF[0], y=ECEF[1], z=ECEF[2]; - double Lat, N, NplusH, delta, esLat; - uint16_t iter; -#define MAX_ITER 100 - - LLA[1] = RAD2DEG*atan2(y,x); - N = a; - NplusH = N; - delta = 1; - Lat = 1; - iter=0; - - while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < MAX_ITER)) - { - delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH)))); - Lat = Lat-delta; - esLat = e*sin(Lat); - N = a / sqrt(1 - esLat*esLat); - NplusH = sqrt(x*x + y*y)/cos(Lat); - iter += 1; - } - - LLA[0] = RAD2DEG*Lat; - LLA[2] = NplusH - N; - - return (iter < MAX_ITER); -} - -// ****** find ECEF to NED rotation matrix ******** -void RneFromLLA(double LLA[3], float Rne[3][3]){ - float sinLat, sinLon, cosLat, cosLon; - - sinLat=(float)sin(DEG2RAD*LLA[0]); - sinLon=(float)sin(DEG2RAD*LLA[1]); - cosLat=(float)cos(DEG2RAD*LLA[0]); - cosLon=(float)cos(DEG2RAD*LLA[1]); - - Rne[0][0] = -sinLat*cosLon; Rne[0][1] = -sinLat*sinLon; Rne[0][2] = cosLat; - 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; -} - -// ****** find roll, pitch, yaw from quaternion ******** -void Quaternion2RPY(float q[4], float rpy[3]){ - float R13, R11, R12, R23, R33; - float q0s=q[0]*q[0]; - float q1s=q[1]*q[1]; - float q2s=q[2]*q[2]; - float q3s=q[3]*q[3]; - - R13 = 2*(q[1]*q[3]-q[0]*q[2]); - R11 = q0s+q1s-q2s-q3s; - R12 = 2*(q[1]*q[2]+q[0]*q[3]); - R23 = 2*(q[2]*q[3]+q[0]*q[1]); - R33 = q0s-q1s-q2s+q3s; - - rpy[1]=RAD2DEG*asinf(-R13); // pitch always between -pi/2 to pi/2 - rpy[2]=RAD2DEG*atan2f(R12,R11); - rpy[0]=RAD2DEG*atan2f(R23,R33); -} - -// ****** find quaternion from roll, pitch, yaw ******** -void RPY2Quaternion(float rpy[3], float q[4]){ - float phi, theta, psi; - float cphi, sphi, ctheta, stheta, cpsi, spsi; - - phi=DEG2RAD*rpy[0]/2; theta=DEG2RAD*rpy[1]/2; psi=DEG2RAD*rpy[2]/2; - cphi=cosf(phi); sphi=sinf(phi); - ctheta=cosf(theta); stheta=sinf(theta); - cpsi=cosf(psi); spsi=sinf(psi); - - q[0] = cphi*ctheta*cpsi + sphi*stheta*spsi; - q[1] = sphi*ctheta*cpsi - cphi*stheta*spsi; - q[2] = cphi*stheta*cpsi + sphi*ctheta*spsi; - q[3] = cphi*ctheta*spsi - sphi*stheta*cpsi; - - if (q[0] < 0){ // q0 always positive for uniqueness - q[0]=-q[0]; - q[1]=-q[1]; - q[2]=-q[2]; - q[3]=-q[3]; - } -} - -//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** -void Quaternion2R(float q[4], float Rbe[3][3]){ - - float q0s=q[0]*q[0], q1s=q[1]*q[1], q2s=q[2]*q[2], q3s=q[3]*q[3]; - - Rbe[0][0]=q0s+q1s-q2s-q3s; - Rbe[0][1]=2*(q[1]*q[2]+q[0]*q[3]); - 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; - Rbe[1][2]=2*(q[2]*q[3]+q[0]*q[1]); - Rbe[2][0]=2*(q[1]*q[3]+q[0]*q[2]); - Rbe[2][1]=2*(q[2]*q[3]-q[0]*q[1]); - Rbe[2][2]=q0s-q1s-q2s+q3s; -} - -// ****** Express LLA in a local NED Base Frame ******** -void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]){ - 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]; -} +/** + ****************************************************************************** + * + * @file CoordinateConversions.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief General conversions with different coordinate systems. + * - all angles in deg + * - distances in meters + * - altitude above WGS-84 elipsoid + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include "CoordinateConversions.h" + +#define RAD2DEG (180.0/M_PI) +#define DEG2RAD (M_PI/180.0) + +// ****** convert Lat,Lon,Alt to ECEF ************ +void LLA2ECEF(double LLA[3], double ECEF[3]) +{ + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double sinLat, sinLon, cosLat, cosLon; + double N; + + sinLat = sin(DEG2RAD * LLA[0]); + sinLon = sin(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 + + ECEF[0] = (N + LLA[2]) * cosLat * cosLon; + 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]) +{ + const double a = 6378137.0; // Equatorial Radius + const double e = 8.1819190842622e-2; // Eccentricity + double x = ECEF[0], y = ECEF[1], z = ECEF[2]; + double Lat, N, NplusH, delta, esLat; + uint16_t iter; +#define MAX_ITER 100 + + LLA[1] = RAD2DEG * atan2(y, x); + N = a; + NplusH = N; + delta = 1; + Lat = 1; + iter = 0; + + while (((delta > 1.0e-14) || (delta < -1.0e-14)) + && (iter < MAX_ITER)) { + delta = + Lat - + atan(z / + (sqrt(x * x + y * y) * + (1 - (N * e * e / NplusH)))); + Lat = Lat - delta; + esLat = e * sin(Lat); + N = a / sqrt(1 - esLat * esLat); + NplusH = sqrt(x * x + y * y) / cos(Lat); + iter += 1; + } + + LLA[0] = RAD2DEG * Lat; + LLA[2] = NplusH - N; + + return (iter < MAX_ITER); +} + +// ****** find ECEF to NED rotation matrix ******** +void RneFromLLA(double LLA[3], float Rne[3][3]) +{ + float sinLat, sinLon, cosLat, cosLon; + + sinLat = (float)sin(DEG2RAD * LLA[0]); + sinLon = (float)sin(DEG2RAD * LLA[1]); + cosLat = (float)cos(DEG2RAD * LLA[0]); + cosLon = (float)cos(DEG2RAD * LLA[1]); + + Rne[0][0] = -sinLat * cosLon; + Rne[0][1] = -sinLat * sinLon; + Rne[0][2] = cosLat; + 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; +} + +// ****** find roll, pitch, yaw from quaternion ******** +void Quaternion2RPY(float q[4], float rpy[3]) +{ + float R13, R11, R12, R23, R33; + float q0s = q[0] * q[0]; + float q1s = q[1] * q[1]; + float q2s = q[2] * q[2]; + float q3s = q[3] * q[3]; + + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; + + rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy[2] = RAD2DEG * atan2f(R12, R11); + rpy[0] = RAD2DEG * atan2f(R23, R33); +} + +// ****** find quaternion from roll, pitch, yaw ******** +void RPY2Quaternion(float rpy[3], float q[4]) +{ + float phi, theta, psi; + float cphi, sphi, ctheta, stheta, cpsi, spsi; + + phi = DEG2RAD * rpy[0] / 2; + theta = DEG2RAD * rpy[1] / 2; + psi = DEG2RAD * rpy[2] / 2; + cphi = cosf(phi); + sphi = sinf(phi); + ctheta = cosf(theta); + stheta = sinf(theta); + cpsi = cosf(psi); + spsi = sinf(psi); + + q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi; + q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi; + q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi; + q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi; + + if (q[0] < 0) { // q0 always positive for uniqueness + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } +} + +//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +void Quaternion2R(float q[4], float Rbe[3][3]) +{ + + float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = + q[2] * q[2], q3s = q[3] * q[3]; + + Rbe[0][0] = q0s + q1s - q2s - q3s; + Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); + 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; + Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]); + Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]); + Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]); + Rbe[2][2] = q0s - q1s - q2s + q3s; +} + +// ****** Express LLA in a local NED Base Frame ******** +void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], + float NED[3]) +{ + 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]; +} diff --git a/flight/Libraries/WorldMagModel.c b/flight/Libraries/WorldMagModel.c index c4dd06e5c..9e7b10394 100644 --- a/flight/Libraries/WorldMagModel.c +++ b/flight/Libraries/WorldMagModel.c @@ -1,1055 +1,1167 @@ -/** - ****************************************************************************** - * - * @file WorldMagModel.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Source file for the World Magnetic Model - * This is a port of code available from the US NOAA. - * The hard coded coefficients should be valid until 2015. - * Major changes include: - * - No geoid model (altitude must be geodetic WGS-84) - * - Floating point calculation (not double precision) - * - Hard coded coefficients for model - * - Elimination of user interface - * - Elimination of dynamic memory allocation - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -// I don't want this dependency, but currently using pvPortMalloc -#include "openpilot.h" - -#include -#include -#include -#include -#include - -#include "WorldMagModel.h" -#include "WMMInternal.h" - -static WMMtype_Ellipsoid * Ellip; -static WMMtype_MagneticModel * MagneticModel; - -/************************************************************************************** -* Example use - very simple - only two exposed functions -* -* 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]); -* 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 -* B is the NED (XYZ) magnetic vector in nTesla -**************************************************************************************/ - -int WMM_Initialize() -// Sets default values for WMM subroutines. -// UPDATES : Ellip and MagneticModel -{ - // Sets WGS-84 parameters - 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->fla = 1/298.257223563; // flattening - Ellip->eps = sqrt(1- (Ellip->b*Ellip->b)/(Ellip->a*Ellip->a )); // first eccentricity - Ellip->epssq = (Ellip->eps*Ellip->eps); // first eccentricity squared - Ellip->re = 6371.2; // Earth's radius in km - - // Sets Magnetic Model parameters - MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel->nMaxSecVar = 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; - MagneticModel->epoch = 2010.0; - sprintf(MagneticModel->ModelName, "WMM-2010"); - 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]) -{ - char Error_Message[255]; - - Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid)); - MagneticModel = (WMMtype_MagneticModel *) pvPortMalloc(sizeof(WMMtype_MagneticModel)); - - WMMtype_CoordSpherical * CoordSpherical = (WMMtype_CoordSpherical *) pvPortMalloc(sizeof(CoordSpherical)); - WMMtype_CoordGeodetic * CoordGeodetic = (WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic)); - WMMtype_Date * Date = (WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date)); - WMMtype_GeoMagneticElements * GeoMagneticElements = (WMMtype_GeoMagneticElements *) pvPortMalloc(sizeof(GeoMagneticElements)); - - WMM_Initialize(); - - CoordGeodetic->lambda = Lon; - CoordGeodetic->phi = Lat; - CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid; - WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/ - - Date->Month=Month; - Date->Day=Day; - Date->Year=Year; - WMM_DateToYear (Date, Error_Message); - WMM_TimelyModifyMagneticModel(Date); - WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/ - - B[0]=GeoMagneticElements->X; - B[1]=GeoMagneticElements->Y; - B[2]=GeoMagneticElements->Z; - - vPortFree(Ellip); - vPortFree(MagneticModel); - vPortFree(CoordSpherical); - vPortFree(CoordGeodetic); - vPortFree(Date); - vPortFree(GeoMagneticElements); -} - -uint16_t WMM_Geomag( WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_GeoMagneticElements *GeoMagneticElements) - /* - 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 - their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid - of magnetic field, these are better achieved by the subroutine WMM_Grid. - - INPUT: Ellip - CoordSpherical - CoordGeodetic - TimedMagneticModel - - OUTPUT : GeoMagneticElements - - CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) - WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF - WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients - WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates - WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements - WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements - - */ -{ - WMMtype_LegendreFunction LegendreFunction; - WMMtype_SphericalHarmonicVariables SphVariables; - WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar; - - WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */ - WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */ - WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ - WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ - WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates*/ - 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; -} - - -uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables) - - /* Computes Spherical variables - Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic - summations. (Equations 10-12 in the WMM Technical Report) - 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 epssq; first eccentricity squared - float eps; first eccentricity - float re; mean radius of ellipsoid - CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model)\ - - 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) - CALLS : none - */ - - { - float cos_lambda, sin_lambda; - uint16_t m, n; - cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); - sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); - /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) - for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); - for (n = 1; n <= nMax; n++) - { - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n-1] * (Ellip->re / CoordSpherical->r); - } - - /* - Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax - cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) - sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) - */ - SphVariables->cos_mlambda[0] = 1.0; - SphVariables->sin_mlambda[0] = 0.0; - - SphVariables->cos_mlambda[1] = cos_lambda; - SphVariables->sin_mlambda[1] = sin_lambda; - for (m = 2; m <= nMax; m++) - { - SphVariables->cos_mlambda[m] = 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; - } - return TRUE; - } /*WMM_ComputeSphericalHarmonicVariables*/ - - -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. - Otherwise WMM_PcupHigh is called. - INPUT CoordSpherical A data structure with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - nMax integer ( Maxumum degree of spherical harmonic secular model) - LegendreFunction Pointer to data structure with the following elements - float *Pcup; ( pointer to store Legendre Function ) - float *dPcup; ( pointer to store Derivative of Lagendre function ) - - OUTPUT LegendreFunction Calculated Legendre variables in the data structure - - */ - - { - float sin_phi; - uint16_t FLAG = 1; - - sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); /* sin (geocentric latitude) */ - - if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10 ) /* If nMax is less tha 16 or at the poles */ - FLAG = WMM_PcupLow(LegendreFunction->Pcup,LegendreFunction->dPcup,sin_phi, nMax); - else FLAG = WMM_PcupHigh(LegendreFunction->Pcup,LegendreFunction->dPcup,sin_phi, nMax); - if (FLAG == 0) /* Error while computing Legendre variables*/ - return FALSE; - - - return TRUE; - } /*WMM_AssociatedLegendreFunction */ - -uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) -{ - /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using - spherical harmonic summation. - - - The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential - The gradient in spherical coordinates is given by: - - dV ^ 1 dV ^ 1 dV ^ - grad V = -- r + - -- t + -------- -- p - dr r dt r sin(t) dp - - - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults - - CALLS : WMM_SummationSpecial - - - - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - */ - uint16_t m, n, index; - float cos_phi; - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m=0;m<=n;m++) - { - index = (n * (n + 1) / 2 + m); - -/* 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] * - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) - * (float) (n+1) * LegendreFunction-> Pcup[index]; - -/* 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)) - 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] * - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] - - MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] ) - * (float) (m) * LegendreFunction-> Pcup[index]; -/* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) - n=1 m=0 n n n */ -/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ - - MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) - * LegendreFunction-> dPcup[index]; - - - - } - } - - cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); - if ( fabs(cos_phi) > 1.0e-10 ) - { - MagneticResults->By = MagneticResults->By / cos_phi ; - } - else - /* Special calculation for component - By - at Geographic poles. - * If the user wants to avoid using this function, please make sure that - * the latitude is not exactly +/-90. An option is to make use the function - * WMM_CheckGeographicPoles. - */ - - { - WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults); - } - return TRUE; -}/*WMM_Summation */ - -uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) -{ - /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. - INPUT : LegendreFunction - MagneticModel - SphVariables - CoordSpherical - OUTPUT : MagneticResults - - CALLS : WMM_SecVarSummationSpecial - - */ - uint16_t m, n, index; - float cos_phi; - MagneticModel->SecularVariationUsed = TRUE; - MagneticResults->Bz = 0.0; - MagneticResults->By = 0.0; - MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - for (m=0;m<=n;m++) - { - index = (n * (n + 1) / 2 + m); - -/* 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.*/ - MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) - * (float) (n+1) * LegendreFunction-> Pcup[index]; - -/* 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)) - n=1 m=0 n n n */ -/* Derivative with respect to longitude, divided by radius. */ - MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] - - MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] ) - * (float) (m) * LegendreFunction-> Pcup[index]; -/* nMax (n+2) n m m m - Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) - n=1 m=0 n n n */ -/* Derivative with respect to latitude, divided by radius. */ - - MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) - * LegendreFunction-> dPcup[index]; - } - } - cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); - if ( fabs(cos_phi) > 1.0e-10 ) - { - MagneticResults->By = MagneticResults->By / cos_phi ; - } - else - /* Special calculation for component By at Geographic poles */ - { - WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, MagneticResults); - } - return TRUE; -} /*WMM_SecVarSummation*/ - -uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults *MagneticResultsGeo) - /* Rotate the Magnetic Vectors to Geodetic Coordinates - Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov - Equation 16, WMM Technical report - - INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements - float lambda; ( longitude) - float phig; ( geocentric latitude ) - float r; ( distance from the center of the ellipsoid) - - CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements - float lambda; (longitude) - float phi; ( geodetic latitude) - float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) - float HeightAboveGeoid;(height above the Geoid ) - - MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements - float Bx; North - float By; East - float Bz; Down - - OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements - float Bx; North - float By; East - float Bz; Down - - CALLS : none - - */ - { - float Psi; - /* Difference between the spherical and Geodetic latitudes */ - Psi = ( M_PI/180 ) * ( CoordSpherical->phig - CoordGeodetic->phi ); - - /* Rotate spherical field components to the Geodeitic system */ - MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); - MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); - MagneticResultsGeo->By = MagneticResultsSph->By; - return TRUE; - } /*WMM_RotateMagneticVector*/ - -uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements) - - /* Calculate all the Geomagnetic elements from X,Y and Z components - INPUT MagneticResultsGeo Pointer to data structure with the following elements - float Bx; ( North ) - float By; ( East ) - float Bz; ( Down ) - OUTPUT GeoMagneticElements Pointer to data structure with the following elements - float Decl; (Angle between the magnetic field vector and true north, positive east) - float Incl; Angle between the magnetic field vector and the horizontal plane, positive down - float F; Magnetic Field Strength - float H; Horizontal Magnetic Field Strength - float X; Northern component of the magnetic field vector - float Y; Eastern component of the magnetic field vector - float Z; Downward component of the magnetic field vector - CALLS : none - */ - { - GeoMagneticElements->X = MagneticResultsGeo->Bx; - GeoMagneticElements->Y = MagneticResultsGeo->By; - GeoMagneticElements->Z = MagneticResultsGeo->Bz; - - GeoMagneticElements->H = sqrt (MagneticResultsGeo->Bx* MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); - GeoMagneticElements->F = sqrt (GeoMagneticElements->H*GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); - GeoMagneticElements->Decl = RAD2DEG(atan2 (GeoMagneticElements->Y , GeoMagneticElements->X)); - GeoMagneticElements->Incl = RAD2DEG(atan2 (GeoMagneticElements->Z , GeoMagneticElements->H)); - - return TRUE; - } /*WMM_CalculateGeoMagneticElements */ - -uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) -/*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. - INPUT MagneticVariation Data structure with the following elements - float Bx; ( North ) - float By; ( East ) - 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 Fdot; Yearly rate of change in Magnetic field strength - float Hdot; Yearly rate of change in horizontal field strength - float Xdot; Yearly rate of change in the northern component - float Ydot; Yearly rate of change in the eastern component - float Zdot; Yearly rate of change in the downward component - float GVdot;Yearly rate of chnage in grid variation - CALLS : none - -*/ -{ - MagneticElements->Xdot = MagneticVariation->Bx; - MagneticElements->Ydot = MagneticVariation->By; - MagneticElements->Zdot = MagneticVariation->Bz; - 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; - MagneticElements->Decldot = 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); - MagneticElements->Incldot = 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); - MagneticElements->GVdot = MagneticElements->Decldot; - return TRUE; -} /*WMM_CalculateSecularVariation*/ - -uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) - -/* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. The functions are initially scaled by - 10^280 sin^m in order to minimize the effects of underflow at large m - near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). - Note that this function performs the same operation as WMM_PcupLow. - However this function also can be used for high degree (large nMax) models. - - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). - - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. - dPcup: Derivative of Pcup(x) with respect to latitude - - CALLS : none - Notes: - - - - Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. - - Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov - - Change from the previous version - The prevous version computes the derivatives as - dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). - However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. - Hence the derivatives are multiplied by sin(latitude). - Removed the options for CS phase and normalizations. - - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. - - The derivates can't be computed for latitude = |90| degrees. - */ - { - float pm2, pm1, pmm, plm, rescalem, z, scalef; - float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP]; - uint16_t k, kstart, m, n; - - if (fabs(x) == 1.0) - { - // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); - return FALSE; - } - - scalef = 1.0e-280; - - for(n = 0 ; n <= 2*nMax+1 ; ++n ) - { - PreSqr[n] = sqrt((float)(n)); - } - - k = 2; - - for(n=2 ; n<=nMax ; n++) - { - k = k + 1; - f1[k] = (float)(2*n-1) /(float)(n); - f2[k] = (float)(n-1) /(float)(n); - for(m=1 ; m<=n-2 ; m++) - { - k = k+1; - f1[k] = (float)(2*n-1) / PreSqr[n+m] / PreSqr[n-m]; - f2[k] = PreSqr[n-m-1] * PreSqr[n+m-1] / PreSqr[n+m] / PreSqr[n-m]; - } - k = k + 2; - } - - /*z = sin (geocentric latitude) */ - z = sqrt((1.0-x)*(1.0+x)); - pm2 = 1.0; - Pcup[0] = 1.0; - dPcup[0] = 0.0; - if (nMax == 0) - return FALSE; - pm1 = x; - Pcup[1] = pm1; - dPcup[1] = z; - k = 1; - - for(n = 2; n <= nMax; n++ ) - { - k = k+n; - plm = f1[k]*x*pm1-f2[k]*pm2; - Pcup[k] = plm; - dPcup[k] = (float)(n) * (pm1 - x * plm) / z; - pm2 = pm1; - pm1 = plm; - } - - pmm = PreSqr[2]*scalef; - rescalem = 1.0/scalef; - kstart = 0; - - for(m = 1; m <= nMax - 1; ++m) - { - rescalem = rescalem*z; - - /* Calculate Pcup(m,m)*/ - kstart = kstart+m+1; - pmm = pmm * PreSqr[2*m+1] / PreSqr[2*m]; - Pcup[kstart] = pmm*rescalem / PreSqr[2*m+1]; - dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); - pm2 = pmm/PreSqr[2*m+1]; - /* Calculate Pcup(m+1,m)*/ - k = kstart+m+1 ; - 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; - /* Calculate Pcup(n,m)*/ - for(n = m+2; n <= nMax; ++n) - { - k = k+n; - plm = x*f1[k]*pm1-f2[k]*pm2; - Pcup[k] = plm*rescalem; - dPcup[k] = (PreSqr[n+m] * PreSqr[n-m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k] ) / z; - pm2 = pm1; - pm1 = plm; - } - } - - /* Calculate Pcup(nMax,nMax)*/ - rescalem = rescalem*z; - kstart = kstart+m+1; - pmm = pmm / PreSqr[2*nMax]; - Pcup[kstart] = pmm * rescalem; - dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; - - return TRUE ; -} /* WMM_PcupHigh */ - -uint16_t WMM_PcupLow( float *Pcup, float *dPcup, float x, uint16_t nMax) - -/* This function evaluates all of the Schmidt-semi normalized associated Legendre - functions up to degree nMax. - - Calling Parameters: - INPUT - nMax: Maximum spherical harmonic degree to compute. - x: cos(colatitude) or sin(latitude). - - OUTPUT - Pcup: A vector of all associated Legendgre polynomials evaluated at - x up to nMax. - dPcup: Derivative of Pcup(x) with respect to latitude - - Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. - Use WMM_PcupHigh for large nMax. - - Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. - - Note: In geomagnetism, the derivatives of ALF are usually found with - respect to the colatitudes. Here the derivatives are found with respect - to the latitude. The difference is a sign reversal for the derivative of - the Associated Legendre Functions. -*/ -{ - uint16_t n, m, index, index1, index2; - float k, z, schmidtQuasiNorm[NUMPCUP]; - Pcup[0] = 1.0; - dPcup[0] = 0.0; - /*sin (geocentric latitude) - sin_phi */ - z = sqrt( ( 1.0 - x ) * ( 1.0 + x ) ) ; - - /* First, Compute the Gauss-normalized associated Legendre functions*/ - for (n = 1; n <= nMax; n++) - { - for (m=0;m<=n;m++) - { - index = (n * (n + 1) / 2 + m); - if (n == m) - { - index1 = ( n - 1 ) * n / 2 + m -1; - Pcup [index] = z * Pcup[index1]; - dPcup[index] = z * dPcup[index1] + x * Pcup[index1]; - } - else if (n == 1 && m == 0) - { - index1 = ( n - 1 ) * n / 2 + m; - Pcup[index] = x * Pcup[index1]; - dPcup[index] = x * dPcup[index1] - z * Pcup[index1]; - } - else if (n > 1 && n != m) - { - index1 = ( n - 2 ) * ( n - 1 ) / 2 + m; - index2 = ( n - 1) * n / 2 + m; - if (m > n - 2) - { - Pcup[index] = x * Pcup[index2]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2]; - } - else - { - k = (float)( ( ( n - 1 ) * ( n - 1 ) ) - ( m * m ) ) / ( float ) ( ( 2 * n - 1 ) * ( 2 * n - 3 ) ); - Pcup[index] = x * Pcup[index2] - k * Pcup[index1]; - dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1]; - } - } - } - } -/*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - schmidtQuasiNorm[0] = 1.0; - for (n = 1; n <= nMax; n++) - { - index = (n * (n + 1) / 2); - index1 = (n - 1) * n / 2 ; - /* for m = 0 */ - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float) (2 * n - 1) / (float) n; - - for ( m = 1; m <= n; m++) - { - index = (n * (n + 1) / 2 + m); - index1 = (n * (n + 1) / 2 + m - 1); - schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt( (float) ((n - m + 1) * (m == 1 ? 2 : 1)) / (float) (n + m)); - } - - } - -/* Converts the Gauss-normalized associated Legendre - functions to the Schmidt quasi-normalized version using pre-computed - relation stored in the variable schmidtQuasiNorm */ - - for (n = 1; n <= nMax; n++) - { - for (m=0;m<=n;m++) - { - index = (n * (n + 1) / 2 + m); - Pcup[index] = Pcup[index] * schmidtQuasiNorm[index]; - dPcup[index] = - dPcup[index] * schmidtQuasiNorm[index]; - /* The sign is changed since the new WMM routines use derivative with respect to latitude - insted of co-latitude */ - } - } - - return TRUE; -} /*WMM_PcupLow */ - - -uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) - /* Special calculation for the component By at Geographic poles. - Manoj Nair, June, 2009 manoj.c.nair@noaa.gov - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report - - */ - { - uint16_t n, index; - float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; - - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0; - - MagneticResults->By = 0.0; - sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - - for (n = 1; n <= MagneticModel->nMax; n++) - { - - /*Compute the ration between the Gauss-normalized associated Legendre - functions and the Schmidt quasi-normalized version. This is equivalent to - sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ - - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt( (float) (n * 2) / (float) (n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n-1]; - } - else - { - 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 - By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) - 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] * - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] - - MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] ) - * PcupS[n] * schmidtQuasiNorm3; - } - - return TRUE; - }/*WMM_SummationSpecial */ - -uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) -{ - /*Special calculation for the secular variation summation at the poles. - - - INPUT: MagneticModel - SphVariables - CoordSpherical - OUTPUT: MagneticResults - CALLS : none - - - */ - uint16_t n, index; - float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3; - - PcupS[0] = 1; - schmidtQuasiNorm1 = 1.0; - - MagneticResults->By = 0.0; - sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) - { - index = (n * (n + 1) / 2 + 1); - schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; - schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt( (float) (n * 2) / (float) (n + 1)); - schmidtQuasiNorm1 = schmidtQuasiNorm2; - if (n == 1) - { - PcupS[n] = PcupS[n-1]; - } - else - { - 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 - By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) - n=1 m=0 n n n */ -/* Derivative with respect to longitude, divided by radius. */ - - MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] - - MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] ) - * PcupS[n] * schmidtQuasiNorm3; - } - - return TRUE; -}/*SecVarSummationSpecial*/ - - -void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate) -// Time change the Model coefficients from the base year of the model using secular variation coefficients. -// -// Modified to work on the global data structure to reduce memory footprint -{ - uint16_t n, m, index, a, b; - - a = MagneticModel->nMaxSecVar; - b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel->nMax; n++) - { - for (m=0;m<=n;m++) - { - index = (n * (n + 1) / 2 + m); - if(index <= b) - { - MagneticModel->Main_Field_Coeff_H[index] += (UserDate->DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; - MagneticModel->Main_Field_Coeff_G[index] += (UserDate->DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; - } - } - } -} /* WMM_TimelyModifyMagneticModel */ - -uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error) -// Converts a given calendar date into a decimal year -{ - uint16_t temp = 0; // Total number of days - uint16_t MonthDays[13] = {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; - uint16_t ExtraDay = 0; - uint16_t i; - - if((CalendarDate->Year%4 == 0 && CalendarDate->Year%100 != 0) || CalendarDate->Year%400 == 0) - ExtraDay=1; - MonthDays[2] += ExtraDay; - - /******************Validation********************************/ - if(CalendarDate->Month <= 0 || CalendarDate->Month > 12) - { - strcpy(Error, "\nError: The Month entered is invalid, valid months are '1 to 12'\n"); - return 0; - } - if(CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) - { - // printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]); - strcpy(Error, "\nError: The day entered is invalid\n"); - return 0; - } - /****************Calculation of t***************************/ - for(i = 1; i <= CalendarDate->Month; i++) - temp+=MonthDays[i-1]; - temp+=CalendarDate->Day; - CalendarDate->DecimalYear = CalendarDate->Year + (temp-1)/(365.0 + ExtraDay); - - return 1; -} /*WMM_DateToYear*/ - -void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) -// Converts Geodetic coordinates to Spherical coordinates -// Convert geodetic coordinates, (defined by the WGS-84 -// reference ellipsoid), to Earth Centered Earth Fixed Cartesian -// coordinates, and then to spherical coordinates. -{ - float CosLat, SinLat, rc, xp, zp; // all local variables - - CosLat = cos(DEG2RAD(CoordGeodetic->phi)); - SinLat = sin(DEG2RAD(CoordGeodetic->phi)); - - // compute the local radius of curvature on the WGS-84 reference ellipsoid - rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat); - - // compute ECEF Cartesian coordinates of specified point (for longitude=0) - - xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; - 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; iMain_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]; - } - -} +/** + ****************************************************************************** + * + * @file WorldMagModel.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Source file for the World Magnetic Model + * This is a port of code available from the US NOAA. + * The hard coded coefficients should be valid until 2015. + * Major changes include: + * - No geoid model (altitude must be geodetic WGS-84) + * - Floating point calculation (not double precision) + * - Hard coded coefficients for model + * - Elimination of user interface + * - Elimination of dynamic memory allocation + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +// I don't want this dependency, but currently using pvPortMalloc +#include "openpilot.h" + +#include +#include +#include +#include +#include + +#include "WorldMagModel.h" +#include "WMMInternal.h" + +static WMMtype_Ellipsoid *Ellip; +static WMMtype_MagneticModel *MagneticModel; + +/************************************************************************************** +* Example use - very simple - only two exposed functions +* +* 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]); +* 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 +* B is the NED (XYZ) magnetic vector in nTesla +**************************************************************************************/ + +int WMM_Initialize() +// Sets default values for WMM subroutines. +// UPDATES : Ellip and MagneticModel +{ + // Sets WGS-84 parameters + 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->fla = 1 / 298.257223563; // flattening + Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity + Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared + Ellip->re = 6371.2; // Earth's radius in km + + // Sets Magnetic Model parameters + MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel->nMaxSecVar = + 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; + MagneticModel->epoch = 2010.0; + sprintf(MagneticModel->ModelName, "WMM-2010"); + 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]) +{ + char Error_Message[255]; + + Ellip = + (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid)); + MagneticModel = + (WMMtype_MagneticModel *) + pvPortMalloc(sizeof(WMMtype_MagneticModel)); + + WMMtype_CoordSpherical *CoordSpherical = + (WMMtype_CoordSpherical *) + pvPortMalloc(sizeof(CoordSpherical)); + WMMtype_CoordGeodetic *CoordGeodetic = + (WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic)); + WMMtype_Date *Date = + (WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date)); + WMMtype_GeoMagneticElements *GeoMagneticElements = + (WMMtype_GeoMagneticElements *) + pvPortMalloc(sizeof(GeoMagneticElements)); + + WMM_Initialize(); + + CoordGeodetic->lambda = Lon; + CoordGeodetic->phi = Lat; + CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid; + WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report */ + + Date->Month = Month; + Date->Day = Day; + Date->Year = Year; + WMM_DateToYear(Date, Error_Message); + WMM_TimelyModifyMagneticModel(Date); + WMM_Geomag(CoordSpherical, CoordGeodetic, GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change */ + + B[0] = GeoMagneticElements->X; + B[1] = GeoMagneticElements->Y; + B[2] = GeoMagneticElements->Z; + + vPortFree(Ellip); + vPortFree(MagneticModel); + vPortFree(CoordSpherical); + vPortFree(CoordGeodetic); + vPortFree(Date); + vPortFree(GeoMagneticElements); +} + +uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, + WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_GeoMagneticElements * GeoMagneticElements) + /* + 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 + their rate of change. Though, this subroutine can be called successively to calculate a time series, profile or grid + of magnetic field, these are better achieved by the subroutine WMM_Grid. + + INPUT: Ellip + CoordSpherical + CoordGeodetic + TimedMagneticModel + + OUTPUT : GeoMagneticElements + + CALLS: WMM_ComputeSphericalHarmonicVariables( Ellip, CoordSpherical, TimedMagneticModel->nMax, &SphVariables); (Compute Spherical Harmonic variables ) + WMM_AssociatedLegendreFunction(CoordSpherical, TimedMagneticModel->nMax, LegendreFunction); Compute ALF + WMM_Summation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSph); Accumulate the spherical harmonic coefficients + WMM_SecVarSummation(LegendreFunction, TimedMagneticModel, SphVariables, CoordSpherical, &MagneticResultsSphVar); Sum the Secular Variation Coefficients + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSph, &MagneticResultsGeo); Map the computed Magnetic fields to Geodeitic coordinates + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, MagneticResultsSphVar, &MagneticResultsGeoVar); Map the secular variation field components to Geodetic coordinates + WMM_CalculateGeoMagneticElements(&MagneticResultsGeo, GeoMagneticElements); Calculate the Geomagnetic elements + WMM_CalculateSecularVariation(MagneticResultsGeoVar, GeoMagneticElements); Calculate the secular variation of each of the Geomagnetic elements + + */ +{ + WMMtype_LegendreFunction LegendreFunction; + WMMtype_SphericalHarmonicVariables SphVariables; + WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, + MagneticResultsSphVar, MagneticResultsGeoVar; + + WMM_ComputeSphericalHarmonicVariables(CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */ + WMM_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */ + WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients */ + WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ + WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSphVar, &MagneticResultsGeoVar); /* Map the secular variation field components to Geodetic coordinates */ + 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; +} + +uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical * + CoordSpherical, + uint16_t nMax, + WMMtype_SphericalHarmonicVariables + * SphVariables) + + /* Computes Spherical variables + Variables computed are (a/r)^(n+2), cos_m(lamda) and sin_m(lambda) for spherical harmonic + summations. (Equations 10-12 in the WMM Technical Report) + 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 epssq; first eccentricity squared + float eps; first eccentricity + float re; mean radius of ellipsoid + CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model)\ + + 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) + CALLS : none + */ +{ + float cos_lambda, sin_lambda; + uint16_t m, n; + cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); + sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); + /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) + for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ + SphVariables->RelativeRadiusPower[0] = + (Ellip->re / CoordSpherical->r) * (Ellip->re / + CoordSpherical->r); + for (n = 1; n <= nMax; n++) { + SphVariables->RelativeRadiusPower[n] = + SphVariables->RelativeRadiusPower[n - + 1] * (Ellip->re / + CoordSpherical-> + r); + } + + /* + Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax + cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) + sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) + */ + SphVariables->cos_mlambda[0] = 1.0; + SphVariables->sin_mlambda[0] = 0.0; + + SphVariables->cos_mlambda[1] = cos_lambda; + SphVariables->sin_mlambda[1] = sin_lambda; + for (m = 2; m <= nMax; m++) { + SphVariables->cos_mlambda[m] = + 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; + } + return TRUE; +} /*WMM_ComputeSphericalHarmonicVariables */ + +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. + Otherwise WMM_PcupHigh is called. + INPUT CoordSpherical A data structure with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + nMax integer ( Maxumum degree of spherical harmonic secular model) + LegendreFunction Pointer to data structure with the following elements + float *Pcup; ( pointer to store Legendre Function ) + float *dPcup; ( pointer to store Derivative of Lagendre function ) + + OUTPUT LegendreFunction Calculated Legendre variables in the data structure + + */ +{ + float sin_phi; + uint16_t FLAG = 1; + + sin_phi = sin(DEG2RAD(CoordSpherical->phig)); /* sin (geocentric latitude) */ + + if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ + FLAG = + WMM_PcupLow(LegendreFunction->Pcup, + LegendreFunction->dPcup, sin_phi, nMax); + else + FLAG = + WMM_PcupHigh(LegendreFunction->Pcup, + LegendreFunction->dPcup, sin_phi, nMax); + if (FLAG == 0) /* Error while computing Legendre variables */ + return FALSE; + + return TRUE; +} /*WMM_AssociatedLegendreFunction */ + +uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction, + WMMtype_SphericalHarmonicVariables * SphVariables, + WMMtype_CoordSpherical * CoordSpherical, + WMMtype_MagneticResults * MagneticResults) +{ + /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using + spherical harmonic summation. + + The vector Magnetic field is given by -grad V, where V is Geomagnetic scalar potential + The gradient in spherical coordinates is given by: + + dV ^ 1 dV ^ 1 dV ^ + grad V = -- r + - -- t + -------- -- p + dr r dt r sin(t) dp + + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults + + CALLS : WMM_SummationSpecial + + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + */ + uint16_t m, n, index; + float cos_phi; + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + +/* 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] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables->cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables->sin_mlambda[m]) + * (float)(n + + 1) * LegendreFunction->Pcup[index]; + +/* 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)) + 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] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables->sin_mlambda[m] - + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; +/* nMax (n+2) n m m m + Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) + n=1 m=0 n n n */ +/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ + + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables->cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + + } + } + + cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else + /* Special calculation for component - By - at Geographic poles. + * If the user wants to avoid using this function, please make sure that + * the latitude is not exactly +/-90. An option is to make use the function + * WMM_CheckGeographicPoles. + */ + + { + WMM_SummationSpecial(SphVariables, CoordSpherical, + MagneticResults); + } + return TRUE; +} /*WMM_Summation */ + +uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, + WMMtype_CoordSpherical * CoordSpherical, + WMMtype_MagneticResults * MagneticResults) +{ + /*This Function sums the secular variation coefficients to get the secular variation of the Magnetic vector. + INPUT : LegendreFunction + MagneticModel + SphVariables + CoordSpherical + OUTPUT : MagneticResults + + CALLS : WMM_SecVarSummationSpecial + + */ + uint16_t m, n, index; + float cos_phi; + MagneticModel->SecularVariationUsed = TRUE; + MagneticResults->Bz = 0.0; + MagneticResults->By = 0.0; + MagneticResults->Bx = 0.0; + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + +/* 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.*/ + MagneticResults->Bz -= + SphVariables->RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables->cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables->sin_mlambda[m]) + * (float)(n + + 1) * LegendreFunction->Pcup[index]; + +/* 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)) + n=1 m=0 n n n */ +/* Derivative with respect to longitude, divided by radius. */ + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables->sin_mlambda[m] - + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables->cos_mlambda[m]) + * (float)(m) * LegendreFunction->Pcup[index]; +/* nMax (n+2) n m m m + Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) + n=1 m=0 n n n */ +/* Derivative with respect to latitude, divided by radius. */ + + MagneticResults->Bx -= + SphVariables->RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables->cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables->sin_mlambda[m]) + * LegendreFunction->dPcup[index]; + } + } + cos_phi = cos(DEG2RAD(CoordSpherical->phig)); + if (fabs(cos_phi) > 1.0e-10) { + MagneticResults->By = MagneticResults->By / cos_phi; + } else + /* Special calculation for component By at Geographic poles */ + { + WMM_SecVarSummationSpecial(SphVariables, CoordSpherical, + MagneticResults); + } + return TRUE; +} /*WMM_SecVarSummation */ + +uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical, + WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_MagneticResults * + MagneticResultsSph, + WMMtype_MagneticResults * + MagneticResultsGeo) + /* Rotate the Magnetic Vectors to Geodetic Coordinates + Manoj Nair, June, 2009 Manoj.C.Nair@Noaa.Gov + Equation 16, WMM Technical report + + INPUT : CoordSpherical : Data structure WMMtype_CoordSpherical with the following elements + float lambda; ( longitude) + float phig; ( geocentric latitude ) + float r; ( distance from the center of the ellipsoid) + + CoordGeodetic : Data structure WMMtype_CoordGeodetic with the following elements + float lambda; (longitude) + float phi; ( geodetic latitude) + float HeightAboveEllipsoid; (height above the ellipsoid (HaE) ) + float HeightAboveGeoid;(height above the Geoid ) + + MagneticResultsSph : Data structure WMMtype_MagneticResults with the following elements + float Bx; North + float By; East + float Bz; Down + + OUTPUT: MagneticResultsGeo Pointer to the data structure WMMtype_MagneticResults, with the following elements + float Bx; North + float By; East + float Bz; Down + + CALLS : none + + */ +{ + float Psi; + /* Difference between the spherical and Geodetic latitudes */ + Psi = (M_PI / 180) * (CoordSpherical->phig - CoordGeodetic->phi); + + /* Rotate spherical field components to the Geodeitic system */ + MagneticResultsGeo->Bz = + MagneticResultsSph->Bx * sin(Psi) + + MagneticResultsSph->Bz * cos(Psi); + MagneticResultsGeo->Bx = + MagneticResultsSph->Bx * cos(Psi) - + MagneticResultsSph->Bz * sin(Psi); + MagneticResultsGeo->By = MagneticResultsSph->By; + return TRUE; +} /*WMM_RotateMagneticVector */ + +uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * + MagneticResultsGeo, + WMMtype_GeoMagneticElements * + GeoMagneticElements) + + /* Calculate all the Geomagnetic elements from X,Y and Z components + INPUT MagneticResultsGeo Pointer to data structure with the following elements + float Bx; ( North ) + float By; ( East ) + float Bz; ( Down ) + OUTPUT GeoMagneticElements Pointer to data structure with the following elements + float Decl; (Angle between the magnetic field vector and true north, positive east) + float Incl; Angle between the magnetic field vector and the horizontal plane, positive down + float F; Magnetic Field Strength + float H; Horizontal Magnetic Field Strength + float X; Northern component of the magnetic field vector + float Y; Eastern component of the magnetic field vector + float Z; Downward component of the magnetic field vector + CALLS : none + */ +{ + GeoMagneticElements->X = MagneticResultsGeo->Bx; + GeoMagneticElements->Y = MagneticResultsGeo->By; + GeoMagneticElements->Z = MagneticResultsGeo->Bz; + + GeoMagneticElements->H = + sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + + MagneticResultsGeo->By * MagneticResultsGeo->By); + GeoMagneticElements->F = + sqrt(GeoMagneticElements->H * GeoMagneticElements->H + + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); + GeoMagneticElements->Decl = + RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); + GeoMagneticElements->Incl = + RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); + + return TRUE; +} /*WMM_CalculateGeoMagneticElements */ + +uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults * + MagneticVariation, + WMMtype_GeoMagneticElements * + MagneticElements) +/*This takes the Magnetic Variation in x, y, and z and uses it to calculate the secular variation of each of the Geomagnetic elements. + INPUT MagneticVariation Data structure with the following elements + float Bx; ( North ) + float By; ( East ) + 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 Fdot; Yearly rate of change in Magnetic field strength + float Hdot; Yearly rate of change in horizontal field strength + float Xdot; Yearly rate of change in the northern component + float Ydot; Yearly rate of change in the eastern component + float Zdot; Yearly rate of change in the downward component + float GVdot;Yearly rate of chnage in grid variation + CALLS : none + +*/ +{ + MagneticElements->Xdot = MagneticVariation->Bx; + MagneticElements->Ydot = MagneticVariation->By; + MagneticElements->Zdot = MagneticVariation->Bz; + 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; + MagneticElements->Decldot = + 180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - + MagneticElements->Y * MagneticElements->Xdot) / + (MagneticElements->H * MagneticElements->H); + MagneticElements->Incldot = + 180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - + MagneticElements->Z * MagneticElements->Hdot) / + (MagneticElements->F * MagneticElements->F); + MagneticElements->GVdot = MagneticElements->Decldot; + return TRUE; +} /*WMM_CalculateSecularVariation */ + +uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax) + +/* This function evaluates all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. The functions are initially scaled by + 10^280 sin^m in order to minimize the effects of underflow at large m + near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). + Note that this function performs the same operation as WMM_PcupLow. + However this function also can be used for high degree (large nMax) models. + + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). + + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. The lenght must by greater or equal to (nMax+1)*(nMax+2)/2. + dPcup: Derivative of Pcup(x) with respect to latitude + + CALLS : none + Notes: + + Adopted from the FORTRAN code written by Mark Wieczorek September 25, 2005. + + Manoj Nair, Nov, 2009 Manoj.C.Nair@Noaa.Gov + + Change from the previous version + The prevous version computes the derivatives as + dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). + However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. + Hence the derivatives are multiplied by sin(latitude). + Removed the options for CS phase and normalizations. + + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. + + The derivates can't be computed for latitude = |90| degrees. + */ +{ + float pm2, pm1, pmm, plm, rescalem, z, scalef; + float f1[NUMPCUP], f2[NUMPCUP], PreSqr[NUMPCUP]; + uint16_t k, kstart, m, n; + + if (fabs(x) == 1.0) { + // printf("Error in PcupHigh: derivative cannot be calculated at poles\n"); + return FALSE; + } + + scalef = 1.0e-280; + + for (n = 0; n <= 2 * nMax + 1; ++n) { + PreSqr[n] = sqrt((float)(n)); + } + + k = 2; + + for (n = 2; n <= nMax; n++) { + k = k + 1; + f1[k] = (float)(2 * n - 1) / (float)(n); + f2[k] = (float)(n - 1) / (float)(n); + for (m = 1; m <= n - 2; m++) { + k = k + 1; + f1[k] = + (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - + m]; + f2[k] = + PreSqr[n - m - 1] * PreSqr[n + m - + 1] / PreSqr[n + + m] / + PreSqr[n - m]; + } + k = k + 2; + } + + /*z = sin (geocentric latitude) */ + z = sqrt((1.0 - x) * (1.0 + x)); + pm2 = 1.0; + Pcup[0] = 1.0; + dPcup[0] = 0.0; + if (nMax == 0) + return FALSE; + pm1 = x; + Pcup[1] = pm1; + dPcup[1] = z; + k = 1; + + for (n = 2; n <= nMax; n++) { + k = k + n; + plm = f1[k] * x * pm1 - f2[k] * pm2; + Pcup[k] = plm; + dPcup[k] = (float)(n) * (pm1 - x * plm) / z; + pm2 = pm1; + pm1 = plm; + } + + pmm = PreSqr[2] * scalef; + rescalem = 1.0 / scalef; + kstart = 0; + + for (m = 1; m <= nMax - 1; ++m) { + rescalem = rescalem * z; + + /* Calculate Pcup(m,m) */ + kstart = kstart + m + 1; + pmm = pmm * PreSqr[2 * m + 1] / PreSqr[2 * m]; + Pcup[kstart] = pmm * rescalem / PreSqr[2 * m + 1]; + dPcup[kstart] = -((float)(m) * x * Pcup[kstart] / z); + pm2 = pmm / PreSqr[2 * m + 1]; + /* Calculate Pcup(m+1,m) */ + k = kstart + m + 1; + 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; + /* Calculate Pcup(n,m) */ + for (n = m + 2; n <= nMax; ++n) { + k = k + n; + plm = x * f1[k] * pm1 - f2[k] * pm2; + Pcup[k] = plm * rescalem; + dPcup[k] = + (PreSqr[n + m] * PreSqr[n - m] * + (pm1 * rescalem) - + (float)(n) * x * Pcup[k]) / z; + pm2 = pm1; + pm1 = plm; + } + } + + /* Calculate Pcup(nMax,nMax) */ + rescalem = rescalem * z; + kstart = kstart + m + 1; + pmm = pmm / PreSqr[2 * nMax]; + Pcup[kstart] = pmm * rescalem; + dPcup[kstart] = -(float)(nMax) * x * Pcup[kstart] / z; + + return TRUE; +} /* WMM_PcupHigh */ + +uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax) + +/* This function evaluates all of the Schmidt-semi normalized associated Legendre + functions up to degree nMax. + + Calling Parameters: + INPUT + nMax: Maximum spherical harmonic degree to compute. + x: cos(colatitude) or sin(latitude). + + OUTPUT + Pcup: A vector of all associated Legendgre polynomials evaluated at + x up to nMax. + dPcup: Derivative of Pcup(x) with respect to latitude + + Notes: Overflow may occur if nMax > 20 , especially for high-latitudes. + Use WMM_PcupHigh for large nMax. + + Writted by Manoj Nair, June, 2009 . Manoj.C.Nair@Noaa.Gov. + + Note: In geomagnetism, the derivatives of ALF are usually found with + respect to the colatitudes. Here the derivatives are found with respect + to the latitude. The difference is a sign reversal for the derivative of + the Associated Legendre Functions. +*/ +{ + uint16_t n, m, index, index1, index2; + float k, z, schmidtQuasiNorm[NUMPCUP]; + Pcup[0] = 1.0; + dPcup[0] = 0.0; + /*sin (geocentric latitude) - sin_phi */ + z = sqrt((1.0 - x) * (1.0 + x)); + + /* First, Compute the Gauss-normalized associated Legendre functions */ + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (n == m) { + index1 = (n - 1) * n / 2 + m - 1; + Pcup[index] = z * Pcup[index1]; + dPcup[index] = + z * dPcup[index1] + x * Pcup[index1]; + } else if (n == 1 && m == 0) { + index1 = (n - 1) * n / 2 + m; + Pcup[index] = x * Pcup[index1]; + dPcup[index] = + x * dPcup[index1] - z * Pcup[index1]; + } else if (n > 1 && n != m) { + index1 = (n - 2) * (n - 1) / 2 + m; + index2 = (n - 1) * n / 2 + m; + if (m > n - 2) { + Pcup[index] = x * Pcup[index2]; + dPcup[index] = + x * dPcup[index2] - + z * Pcup[index2]; + } else { + k = (float)(((n - 1) * (n - 1)) - + (m * m)) / (float)((2 * + n - + 1) + * + (2 * + n - + 3)); + Pcup[index] = + x * Pcup[index2] - + k * Pcup[index1]; + dPcup[index] = + x * dPcup[index2] - + z * Pcup[index2] - + k * dPcup[index1]; + } + } + } + } +/*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + + schmidtQuasiNorm[0] = 1.0; + for (n = 1; n <= nMax; n++) { + index = (n * (n + 1) / 2); + index1 = (n - 1) * n / 2; + /* for m = 0 */ + schmidtQuasiNorm[index] = + schmidtQuasiNorm[index1] * (float)(2 * n - + 1) / (float)n; + + for (m = 1; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + index1 = (n * (n + 1) / 2 + m - 1); + schmidtQuasiNorm[index] = + schmidtQuasiNorm[index1] * + sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) / + (float)(n + m)); + } + + } + +/* Converts the Gauss-normalized associated Legendre + functions to the Schmidt quasi-normalized version using pre-computed + relation stored in the variable schmidtQuasiNorm */ + + for (n = 1; n <= nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + Pcup[index] = + Pcup[index] * schmidtQuasiNorm[index]; + dPcup[index] = + -dPcup[index] * schmidtQuasiNorm[index]; + /* The sign is changed since the new WMM routines use derivative with respect to latitude + insted of co-latitude */ + } + } + + return TRUE; +} /*WMM_PcupLow */ + +uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * + SphVariables, + WMMtype_CoordSpherical * CoordSpherical, + WMMtype_MagneticResults * MagneticResults) + /* Special calculation for the component By at Geographic poles. + Manoj Nair, June, 2009 manoj.c.nair@noaa.gov + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + See Section 1.4, "SINGULARITIES AT THE GEOGRAPHIC POLES", WMM Technical report + + */ +{ + uint16_t n, index; + float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, + schmidtQuasiNorm2, schmidtQuasiNorm3; + + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0; + + MagneticResults->By = 0.0; + sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + + for (n = 1; n <= MagneticModel->nMax; n++) { + + /*Compute the ration between the Gauss-normalized associated Legendre + functions and the Schmidt quasi-normalized version. This is equivalent to + sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ + + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = + schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = + schmidtQuasiNorm2 * sqrt((float)(n * 2) / + (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + 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 + By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) + 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] * + (MagneticModel->Main_Field_Coeff_G[index] * + SphVariables->sin_mlambda[1] - + MagneticModel->Main_Field_Coeff_H[index] * + SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } + + return TRUE; +} /*WMM_SummationSpecial */ + +uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * + SphVariables, + WMMtype_CoordSpherical * + CoordSpherical, + WMMtype_MagneticResults * + MagneticResults) +{ + /*Special calculation for the secular variation summation at the poles. + + INPUT: MagneticModel + SphVariables + CoordSpherical + OUTPUT: MagneticResults + CALLS : none + + */ + uint16_t n, index; + float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, + schmidtQuasiNorm2, schmidtQuasiNorm3; + + PcupS[0] = 1; + schmidtQuasiNorm1 = 1.0; + + MagneticResults->By = 0.0; + sin_phi = sin(DEG2RAD(CoordSpherical->phig)); + + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { + index = (n * (n + 1) / 2 + 1); + schmidtQuasiNorm2 = + schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; + schmidtQuasiNorm3 = + schmidtQuasiNorm2 * sqrt((float)(n * 2) / + (float)(n + 1)); + schmidtQuasiNorm1 = schmidtQuasiNorm2; + if (n == 1) { + PcupS[n] = PcupS[n - 1]; + } else { + 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 + By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) + n=1 m=0 n n n */ +/* Derivative with respect to longitude, divided by radius. */ + + MagneticResults->By += + SphVariables->RelativeRadiusPower[n] * + (MagneticModel->Secular_Var_Coeff_G[index] * + SphVariables->sin_mlambda[1] - + MagneticModel->Secular_Var_Coeff_H[index] * + SphVariables->cos_mlambda[1]) + * PcupS[n] * schmidtQuasiNorm3; + } + + return TRUE; +} /*SecVarSummationSpecial */ + +void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate) +// Time change the Model coefficients from the base year of the model using secular variation coefficients. +// +// Modified to work on the global data structure to reduce memory footprint +{ + uint16_t n, m, index, a, b; + + a = MagneticModel->nMaxSecVar; + b = (a * (a + 1) / 2 + a); + for (n = 1; n <= MagneticModel->nMax; n++) { + for (m = 0; m <= n; m++) { + index = (n * (n + 1) / 2 + m); + if (index <= b) { + MagneticModel->Main_Field_Coeff_H[index] += + (UserDate->DecimalYear - + MagneticModel->epoch) * + MagneticModel-> + Secular_Var_Coeff_H[index]; + MagneticModel->Main_Field_Coeff_G[index] += + (UserDate->DecimalYear - + MagneticModel->epoch) * + MagneticModel-> + Secular_Var_Coeff_G[index]; + } + } + } +} /* WMM_TimelyModifyMagneticModel */ + +uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error) +// Converts a given calendar date into a decimal year +{ + uint16_t temp = 0; // Total number of days + uint16_t MonthDays[13] = + { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + uint16_t ExtraDay = 0; + uint16_t i; + + if ((CalendarDate->Year % 4 == 0 && CalendarDate->Year % 100 != 0) + || CalendarDate->Year % 400 == 0) + ExtraDay = 1; + MonthDays[2] += ExtraDay; + + /******************Validation********************************/ + if (CalendarDate->Month <= 0 || CalendarDate->Month > 12) { + strcpy(Error, + "\nError: The Month entered is invalid, valid months are '1 to 12'\n"); + return 0; + } + if (CalendarDate->Day <= 0 + || CalendarDate->Day > MonthDays[CalendarDate->Month]) { + // printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]); + strcpy(Error, "\nError: The day entered is invalid\n"); + return 0; + } + /****************Calculation of t***************************/ + for (i = 1; i <= CalendarDate->Month; i++) + temp += MonthDays[i - 1]; + temp += CalendarDate->Day; + CalendarDate->DecimalYear = + CalendarDate->Year + (temp - 1) / (365.0 + ExtraDay); + + return 1; +} /*WMM_DateToYear */ + +void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_CoordSpherical * CoordSpherical) +// Converts Geodetic coordinates to Spherical coordinates +// Convert geodetic coordinates, (defined by the WGS-84 +// reference ellipsoid), to Earth Centered Earth Fixed Cartesian +// coordinates, and then to spherical coordinates. +{ + float CosLat, SinLat, rc, xp, zp; // all local variables + + CosLat = cos(DEG2RAD(CoordGeodetic->phi)); + SinLat = sin(DEG2RAD(CoordGeodetic->phi)); + + // compute the local radius of curvature on the WGS-84 reference ellipsoid + rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat); + + // compute ECEF Cartesian coordinates of specified point (for longitude=0) + + xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + 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]; + } + +} diff --git a/flight/Libraries/buffer.c b/flight/Libraries/buffer.c index 021210a67..d9fd23725 100644 --- a/flight/Libraries/buffer.c +++ b/flight/Libraries/buffer.c @@ -1,244 +1,238 @@ -/** - ****************************************************************************** - * - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Flight_Libraries Miscellaneous library functions - * @brief Miscellaneous library functions shared between PIOS / OpenPilot / AHRS - * @{ - * - * @file buffer.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Simplies buffering data - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ - -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -//***************************************************************************** -// -// File Name : 'buffer.c' -// Title : Multipurpose byte buffer structure and methods -// Author : Pascal Stang - Copyright (C) 2001-2002 -// Created : 9/23/2001 -// Revised : 9/23/2001 -// Version : 1.0 -// Target MCU : any -// Editor Tabs : 4 -// -// This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt -// -//***************************************************************************** - -#include "buffer.h" - -/** - * @brief Initialize a cBuffer structure - * @param[in] buffer Points to the buffer structure - * @param[in] start Allocated memory to store data - * @param[in] size Maximum size of buffer - * @return None - */ -void bufferInit(cBuffer* buffer, uint8_t *start, uint32_t size) -{ - // set start pointer of the buffer - buffer->dataptr = start; - buffer->size = size; - // initialize index and length - buffer->dataindex = 0; - buffer->datalength = 0; -} - -/** - * @brief Return remaining space in buffer - * @param[in] buffer Pointer to buffer structure - * @return Amount of space remaining on buffer - */ -uint32_t bufferRemainingSpace(cBuffer* buffer) -{ - return buffer->size - buffer->datalength; -} - -/** - * @brief Return amount of data - * @param[in] buffer Pointer to buffer structure - * @return Amount of data queued in buffer - */ -uint32_t bufferBufferedData(cBuffer* buffer) -{ - return buffer->datalength; -} - - -/** - * @brief Pop one element from buffer - * @param[in] buffer Pointer to the buffer structure - * @return None - */ -uint8_t bufferGetFromFront(cBuffer* buffer) -{ - unsigned char data = 0; - - // check to see if there's data in the buffer - if(buffer->datalength) - { - // get the first character from buffer - data = buffer->dataptr[buffer->dataindex]; - // move index down and decrement length - buffer->dataindex++; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength--; - } - // 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 - * @param[in] size Number of elements to get - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferGetChunkFromFront(cBuffer* buffer, uint8_t * dest, uint32_t size) -{ - if(size > buffer->datalength) - return -1; - - for(uint32_t i = 0; i < size; i++) - { - dest[i] = bufferGetFromFront(buffer); - } - - 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 - * @return None - */ -void bufferDumpFromFront(cBuffer* buffer, uint32_t numbytes) -{ - // dump numbytes from the front of the buffer - // are we dumping less than the entire buffer? - if(numbytes < buffer->datalength) - { - // move index down by numbytes and decrement length by numbytes - buffer->dataindex += numbytes; - if(buffer->dataindex >= buffer->size) - { - buffer->dataindex %= buffer->size; - } - buffer->datalength -= numbytes; - } - else - { - // flush the whole buffer - buffer->datalength = 0; - } -} - -/** - * @brief Get element indexed from the front of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] index Index into the buffer relative to front - * @return None - */ -uint8_t bufferGetAtIndex(cBuffer* buffer, uint32_t index) -{ - // return character at index in buffer - return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)]; -} - -/** - * @brief Queue a character to end of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] data Byte to add - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferAddToEnd(cBuffer* buffer, uint8_t data) -{ - // make sure the buffer has room - if(buffer->datalength < buffer->size) - { - // save data byte at end of buffer - buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data; - // increment the length - buffer->datalength++; - // return success - return -1; - } - else return 0; -} - -/** - * @brief Queue a block of character to end of buffer - * @param[in] buffer Point to the buffer structure - * @param[in] data Pointer to data to add - * @param[in] size Number of bytes to add - * @return - * @arg -1 for success - * @arg 0 error - */ -uint8_t bufferAddChunkToEnd(cBuffer* buffer, const uint8_t * data, uint32_t size) -{ - // TODO: replace with memcpy and logic, for now keeping it simple - for(uint32_t i = 0; i < size; i++) - { - if(bufferAddToEnd(buffer,data[i]) == 0) - return 0; - } - return -1; -} - -/** - * @brief Check to see if the buffer has room - * @param[in] buffer Point to the buffer structure - * @return - * @arg True there is room available in buffer - * @arg False buffer is full - */ -unsigned char bufferIsNotFull(cBuffer* buffer) -{ - return (buffer->datalength < buffer->size); -} - -/** - * @brief Trash all data in buffer - * @param[in] buffer Point to the buffer structure - */ -void bufferFlush(cBuffer* buffer) -{ - // flush contents of the buffer - buffer->datalength = 0; -} - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Flight_Libraries Miscellaneous library functions + * @brief Miscellaneous library functions shared between PIOS / OpenPilot / AHRS + * @{ + * + * @file buffer.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Simplies buffering data + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ + +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +//***************************************************************************** +// +// File Name : 'buffer.c' +// Title : Multipurpose byte buffer structure and methods +// Author : Pascal Stang - Copyright (C) 2001-2002 +// Created : 9/23/2001 +// Revised : 9/23/2001 +// Version : 1.0 +// Target MCU : any +// Editor Tabs : 4 +// +// This code is distributed under the GNU Public License +// which can be found at http://www.gnu.org/licenses/gpl.txt +// +//***************************************************************************** + +#include "buffer.h" + +/** + * @brief Initialize a cBuffer structure + * @param[in] buffer Points to the buffer structure + * @param[in] start Allocated memory to store data + * @param[in] size Maximum size of buffer + * @return None + */ +void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size) +{ + // set start pointer of the buffer + buffer->dataptr = start; + buffer->size = size; + // initialize index and length + buffer->dataindex = 0; + buffer->datalength = 0; +} + +/** + * @brief Return remaining space in buffer + * @param[in] buffer Pointer to buffer structure + * @return Amount of space remaining on buffer + */ +uint32_t bufferRemainingSpace(cBuffer * buffer) +{ + return buffer->size - buffer->datalength; +} + +/** + * @brief Return amount of data + * @param[in] buffer Pointer to buffer structure + * @return Amount of data queued in buffer + */ +uint32_t bufferBufferedData(cBuffer * buffer) +{ + return buffer->datalength; +} + +/** + * @brief Pop one element from buffer + * @param[in] buffer Pointer to the buffer structure + * @return None + */ +uint8_t bufferGetFromFront(cBuffer * buffer) +{ + unsigned char data = 0; + + // check to see if there's data in the buffer + if (buffer->datalength) { + // get the first character from buffer + data = buffer->dataptr[buffer->dataindex]; + // move index down and decrement length + buffer->dataindex++; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength--; + } + // 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 + * @param[in] size Number of elements to get + * @return + * @arg -1 for success + * @arg 0 error + */ +uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, + uint32_t size) +{ + if (size > buffer->datalength) + return -1; + + for (uint32_t i = 0; i < size; i++) { + dest[i] = bufferGetFromFront(buffer); + } + + 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 + * @return None + */ +void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes) +{ + // dump numbytes from the front of the buffer + // are we dumping less than the entire buffer? + if (numbytes < buffer->datalength) { + // move index down by numbytes and decrement length by numbytes + buffer->dataindex += numbytes; + if (buffer->dataindex >= buffer->size) { + buffer->dataindex %= buffer->size; + } + buffer->datalength -= numbytes; + } else { + // flush the whole buffer + buffer->datalength = 0; + } +} + +/** + * @brief Get element indexed from the front of buffer + * @param[in] buffer Point to the buffer structure + * @param[in] index Index into the buffer relative to front + * @return None + */ +uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index) +{ + // return character at index in buffer + return buffer->dataptr[(buffer->dataindex + index) % + (buffer->size)]; +} + +/** + * @brief Queue a character to end of buffer + * @param[in] buffer Point to the buffer structure + * @param[in] data Byte to add + * @return + * @arg -1 for success + * @arg 0 error + */ +uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data) +{ + // make sure the buffer has room + if (buffer->datalength < buffer->size) { + // save data byte at end of buffer + buffer->dataptr[(buffer->dataindex + buffer->datalength) % + buffer->size] = data; + // increment the length + buffer->datalength++; + // return success + return -1; + } else + return 0; +} + +/** + * @brief Queue a block of character to end of buffer + * @param[in] buffer Point to the buffer structure + * @param[in] data Pointer to data to add + * @param[in] size Number of bytes to add + * @return + * @arg -1 for success + * @arg 0 error + */ +uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data, + uint32_t size) +{ + // TODO: replace with memcpy and logic, for now keeping it simple + for (uint32_t i = 0; i < size; i++) { + if (bufferAddToEnd(buffer, data[i]) == 0) + return 0; + } + return -1; +} + +/** + * @brief Check to see if the buffer has room + * @param[in] buffer Point to the buffer structure + * @return + * @arg True there is room available in buffer + * @arg False buffer is full + */ +unsigned char bufferIsNotFull(cBuffer * buffer) +{ + return (buffer->datalength < buffer->size); +} + +/** + * @brief Trash all data in buffer + * @param[in] buffer Point to the buffer structure + */ +void bufferFlush(cBuffer * buffer) +{ + // flush contents of the buffer + buffer->datalength = 0; +} + +/** + * @} + * @} + */ diff --git a/flight/Libraries/inc/CoordinateConversions.h b/flight/Libraries/inc/CoordinateConversions.h index e5b832eb0..3cc308cd0 100644 --- a/flight/Libraries/inc/CoordinateConversions.h +++ b/flight/Libraries/inc/CoordinateConversions.h @@ -1,56 +1,58 @@ -/** - ****************************************************************************** - * - * @file CoordinateConverions.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Header for Coordinate conversions library in CoordinateConversions.c - * - all angles in deg - * - distances in meters - * - altitude above WGS-84 elipsoid - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef COORDINATECONVERSIONS_H_ -#define COORDINATECONVERSIONS_H_ - - // ****** convert Lat,Lon,Alt to ECEF ************ - void LLA2ECEF(double LLA[3], double ECEF[3]); - - // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* - uint16_t ECEF2LLA(double ECEF[3], double LLA[3]); - - void RneFromLLA(double LLA[3], float Rne[3][3]); - - // ****** find roll, pitch, yaw from quaternion ******** - void Quaternion2RPY(float q[4], float rpy[3]); - - // ****** find quaternion from roll, pitch, yaw ******** - void RPY2Quaternion(float rpy[3], float q[4]); - - //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** - void Quaternion2R(float q[4], float Rbe[3][3]); - - // ****** Express LLA in a local NED Base Frame ******** - 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]); - -#endif // COORDINATECONVERSIONS_H_ +/** + ****************************************************************************** + * + * @file CoordinateConverions.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Header for Coordinate conversions library in CoordinateConversions.c + * - all angles in deg + * - distances in meters + * - altitude above WGS-84 elipsoid + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef COORDINATECONVERSIONS_H_ +#define COORDINATECONVERSIONS_H_ + + // ****** convert Lat,Lon,Alt to ECEF ************ +void LLA2ECEF(double LLA[3], double ECEF[3]); + + // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) ********* +uint16_t ECEF2LLA(double ECEF[3], double LLA[3]); + +void RneFromLLA(double LLA[3], float Rne[3][3]); + + // ****** find roll, pitch, yaw from quaternion ******** +void Quaternion2RPY(float q[4], float rpy[3]); + + // ****** find quaternion from roll, pitch, yaw ******** +void RPY2Quaternion(float rpy[3], float q[4]); + + //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +void Quaternion2R(float q[4], float Rbe[3][3]); + + // ****** Express LLA in a local NED Base Frame ******** +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]); + +#endif // COORDINATECONVERSIONS_H_ diff --git a/flight/Libraries/inc/WMMInternal.h b/flight/Libraries/inc/WMMInternal.h index 8e68ecded..80b7fac2e 100644 --- a/flight/Libraries/inc/WMMInternal.h +++ b/flight/Libraries/inc/WMMInternal.h @@ -1,168 +1,185 @@ -/** - ****************************************************************************** - * - * @file WMMInternal.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the WorldMagModel internal functionality. - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef WMMINTERNAL_H_ -#define WMMINTERNAL_H_ - - // internal constants - #define TRUE ((uint16_t)1) - #define FALSE ((uint16_t)0) - #define WMM_MAX_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 NUMPCUP 92 // NUMTERMS +1 - #define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 - #define RAD2DEG(rad) ((rad)*(180.0L/M_PI)) - #define DEG2RAD(deg) ((deg)*(M_PI/180.0L)) - - - // internal structure definitions - typedef struct { - float EditionDate; - float epoch; //Base time of Geomagnetic model epoch (yrs) - char ModelName[20]; - 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 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) - uint16_t nMax; // Maximum degree of spherical harmonic 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 - } WMMtype_MagneticModel; - - typedef struct { - float a; // semi-major axis of the ellipsoid - float b; // semi-minor axis of the ellipsoid - float fla; // flattening - float epssq; // first eccentricity squared - float eps; // first eccentricity - float re; // mean radius of ellipsoid - } WMMtype_Ellipsoid; - - typedef struct { - float lambda; // longitude - float phi; // geodetic latitude - float HeightAboveEllipsoid; // height above the ellipsoid (HaE) - } WMMtype_CoordGeodetic; - - typedef struct { - float lambda; // longitude - float phig; // geocentric latitude - float r; // distance from the center of the ellipsoid - } WMMtype_CoordSpherical; - - typedef struct { - uint16_t Year; - uint16_t Month; - uint16_t Day; - float DecimalYear; - } WMMtype_Date; - - typedef struct { - float Pcup[NUMPCUP]; // Legendre Function - float dPcup[NUMPCUP]; // Derivative of Lagendre fn - } WMMtype_LegendreFunction; - - typedef struct { - float Bx; // North - float By; // East - float Bz; // Down - } WMMtype_MagneticResults; - - typedef struct { - - 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 sin_mlambda[WMM_MAX_MODEL_DEGREES+1]; // sp(m) - sine of (m*spherical coord. longitude) - } WMMtype_SphericalHarmonicVariables; - - typedef struct { - 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 F; /*3. Magnetic Field Strength*/ - float H; /*4. Horizontal Magnetic Field Strength*/ - float X; /*5. Northern 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 GV; /*8. The Grid Variation*/ - float Decldot; /*9. Yearly Rate of change in declination*/ - float Incldot; /*10. Yearly Rate of change in inclination*/ - float Fdot; /*11. Yearly rate of change in Magnetic 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 Ydot; /*14. Yearly rate of change in the eastern component*/ - float Zdot; /*15. Yearly rate of change in the downward component*/ - float GVdot; /*16. Yearly rate of chnage in grid variation*/ - } WMMtype_GeoMagneticElements; - - // Internal Function Prototypes - void WMM_Set_Coeff_Array(); - void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); - uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error); - void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate); - uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_GeoMagneticElements *GeoMagneticElements); - - uint16_t WMM_AssociatedLegendreFunction( WMMtype_CoordSpherical * CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction); - - uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResultsGeo, WMMtype_GeoMagneticElements *GeoMagneticElements); - - uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements); - - uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_CoordSpherical *CoordSpherical, - uint16_t nMax, - WMMtype_SphericalHarmonicVariables * SphVariables); - - uint16_t WMM_PcupLow( float *Pcup, float *dPcup, float x, uint16_t nMax); - - uint16_t WMM_PcupHigh( float *Pcup, float *dPcup, float x, uint16_t nMax); - - uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * , - WMMtype_CoordGeodetic * CoordGeodetic, - WMMtype_MagneticResults * MagneticResultsSph, - WMMtype_MagneticResults *MagneticResultsGeo); - - uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, - WMMtype_SphericalHarmonicVariables * SphVariables, - WMMtype_CoordSpherical * CoordSpherical, - WMMtype_MagneticResults *MagneticResults); - - 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_ */ \ No newline at end of file +/** + ****************************************************************************** + * + * @file WMMInternal.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the WorldMagModel internal functionality. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef WMMINTERNAL_H_ +#define WMMINTERNAL_H_ + + // internal constants +#define TRUE ((uint16_t)1) +#define FALSE ((uint16_t)0) +#define WMM_MAX_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 NUMPCUP 92 // NUMTERMS +1 +#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 +#define RAD2DEG(rad) ((rad)*(180.0L/M_PI)) +#define DEG2RAD(deg) ((deg)*(M_PI/180.0L)) + + // internal structure definitions +typedef struct { + float EditionDate; + float epoch; //Base time of Geomagnetic model epoch (yrs) + char ModelName[20]; + 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 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) + uint16_t nMax; // Maximum degree of spherical harmonic 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 +} WMMtype_MagneticModel; + +typedef struct { + float a; // semi-major axis of the ellipsoid + float b; // semi-minor axis of the ellipsoid + float fla; // flattening + float epssq; // first eccentricity squared + float eps; // first eccentricity + float re; // mean radius of ellipsoid +} WMMtype_Ellipsoid; + +typedef struct { + float lambda; // longitude + float phi; // geodetic latitude + float HeightAboveEllipsoid; // height above the ellipsoid (HaE) +} WMMtype_CoordGeodetic; + +typedef struct { + float lambda; // longitude + float phig; // geocentric latitude + float r; // distance from the center of the ellipsoid +} WMMtype_CoordSpherical; + +typedef struct { + uint16_t Year; + uint16_t Month; + uint16_t Day; + float DecimalYear; +} WMMtype_Date; + +typedef struct { + float Pcup[NUMPCUP]; // Legendre Function + float dPcup[NUMPCUP]; // Derivative of Lagendre fn +} WMMtype_LegendreFunction; + +typedef struct { + float Bx; // North + float By; // East + float Bz; // Down +} WMMtype_MagneticResults; + +typedef struct { + + 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 sin_mlambda[WMM_MAX_MODEL_DEGREES + 1]; // sp(m) - sine of (m*spherical coord. longitude) +} WMMtype_SphericalHarmonicVariables; + +typedef struct { + 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 F; /*3. Magnetic Field Strength */ + float H; /*4. Horizontal Magnetic Field Strength */ + float X; /*5. Northern 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 GV; /*8. The Grid Variation */ + float Decldot; /*9. Yearly Rate of change in declination */ + float Incldot; /*10. Yearly Rate of change in inclination */ + float Fdot; /*11. Yearly rate of change in Magnetic 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 Ydot; /*14. Yearly rate of change in the eastern component */ + float Zdot; /*15. Yearly rate of change in the downward component */ + float GVdot; /*16. Yearly rate of chnage in grid variation */ +} WMMtype_GeoMagneticElements; + + // Internal Function Prototypes +void WMM_Set_Coeff_Array(); +void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_CoordSpherical * CoordSpherical); +uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error); +void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate); +uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, + WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_GeoMagneticElements * GeoMagneticElements); + +uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * + CoordSpherical, uint16_t nMax, + WMMtype_LegendreFunction * + LegendreFunction); + +uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * + MagneticResultsGeo, + WMMtype_GeoMagneticElements * + GeoMagneticElements); + +uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults * + MagneticVariation, + WMMtype_GeoMagneticElements * + MagneticElements); + +uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical * + CoordSpherical, + uint16_t nMax, + WMMtype_SphericalHarmonicVariables + * SphVariables); + +uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax); + +uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax); + +uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical *, + WMMtype_CoordGeodetic * CoordGeodetic, + WMMtype_MagneticResults * + MagneticResultsSph, + WMMtype_MagneticResults * + MagneticResultsGeo); + +uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction, + WMMtype_SphericalHarmonicVariables * + SphVariables, + WMMtype_CoordSpherical * CoordSpherical, + WMMtype_MagneticResults * MagneticResults); + +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_ */ diff --git a/flight/Libraries/inc/WorldMagModel.h b/flight/Libraries/inc/WorldMagModel.h index 9d7521257..b3766a0c6 100644 --- a/flight/Libraries/inc/WorldMagModel.h +++ b/flight/Libraries/inc/WorldMagModel.h @@ -1,34 +1,36 @@ -/** - ****************************************************************************** - * - * @file WorldMagModel.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the WorldMagModel exposed functionality. - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef WORLDMAGMODEL_H_ -#define WORLDMAGMODEL_H_ - - // Exposed Function Prototypes - int WMM_Initialize(); - void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]); - -#endif /* WORLDMAGMODEL_H_ */ \ No newline at end of file +/** + ****************************************************************************** + * + * @file WorldMagModel.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the WorldMagModel exposed functionality. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef WORLDMAGMODEL_H_ +#define WORLDMAGMODEL_H_ + + // Exposed Function Prototypes +int WMM_Initialize(); +void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, + uint16_t Month, uint16_t Day, uint16_t Year, + float B[3]); + +#endif /* WORLDMAGMODEL_H_ */ diff --git a/flight/Libraries/inc/buffer.h b/flight/Libraries/inc/buffer.h index 6eeac2e28..a9c185bcf 100644 --- a/flight/Libraries/inc/buffer.h +++ b/flight/Libraries/inc/buffer.h @@ -1,106 +1,107 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file buffer.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief see below - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ - -//***************************************************************************** -// -// File Name : 'buffer.h' -// Title : Multipurpose byte buffer structure and methods -// Author : Pascal Stang - Copyright (C) 2001-2002 -// Created : 9/23/2001 -// Revised : 11/16/2002 -// Version : 1.1 -// Target MCU : any -// Editor Tabs : 4 -// -/// \code #include "buffer.h" \endcode -/// \par Overview -/// 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 -/// like (within memory limits), and then use this common set of functions to -/// 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 -/// 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 -/// 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. -/// 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. -// -// This code is distributed under the GNU Public License -// which can be found at http://www.gnu.org/licenses/gpl.txt -// -//***************************************************************************** -//@{ - -#ifndef BUFFER_H -#define BUFFER_H - -#include "stdint.h" - -// structure/typdefs - -//! cBuffer structure -typedef struct struct_cBuffer -{ - unsigned char *dataptr; ///< the physical memory address where the buffer is stored - unsigned short size; ///< the allocated size of 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 -} cBuffer; - -// function prototypes - -//! initialize a buffer to start at a given address and have given size -void bufferInit(cBuffer* buffer, uint8_t *start, uint32_t size); - -//! check free space in buffer -uint32_t bufferRemainingSpace(cBuffer* buffer); - -//! get the first byte from the front of the buffer -uint8_t bufferGetFromFront(cBuffer* buffer); - -//! get the number of bytes buffered -uint32_t bufferBufferedData(cBuffer* buffer); - -//! copy number of elements into another buffer -uint8_t bufferGetChunkFromFront(cBuffer* buffer, uint8_t * dest, uint32_t size); - -//! dump (discard) the first numbytes from the front of the buffer -void bufferDumpFromFront(cBuffer* buffer, uint32_t numbytes); - -//! 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 -uint8_t bufferGetAtIndex(cBuffer* buffer, uint32_t index); - -//! add a byte to the end of the buffer -uint8_t bufferAddToEnd(cBuffer* buffer, uint8_t data); - -//! queue a block of character to end of buffer -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); - -//! flush (clear) the contents of the buffer -void bufferFlush(cBuffer* buffer); - -#endif - -/** - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file buffer.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief see below + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and + * objects. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ + +//***************************************************************************** +// +// File Name : 'buffer.h' +// Title : Multipurpose byte buffer structure and methods +// Author : Pascal Stang - Copyright (C) 2001-2002 +// Created : 9/23/2001 +// Revised : 11/16/2002 +// Version : 1.1 +// Target MCU : any +// Editor Tabs : 4 +// +/// \code #include "buffer.h" \endcode +/// \par Overview +/// 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 +/// like (within memory limits), and then use this common set of functions to +/// 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 +/// 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 +/// 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. +/// 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. +// +// This code is distributed under the GNU Public License +// which can be found at http://www.gnu.org/licenses/gpl.txt +// +//***************************************************************************** +//@{ + +#ifndef BUFFER_H +#define BUFFER_H + +#include "stdint.h" + +// structure/typdefs + +//! cBuffer structure +typedef struct struct_cBuffer { + unsigned char *dataptr; ///< the physical memory address where the buffer is stored + unsigned short size; ///< the allocated size of 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 +} cBuffer; + +// function prototypes + +//! initialize a buffer to start at a given address and have given size +void bufferInit(cBuffer * buffer, uint8_t * start, uint32_t size); + +//! check free space in buffer +uint32_t bufferRemainingSpace(cBuffer * buffer); + +//! get the first byte from the front of the buffer +uint8_t bufferGetFromFront(cBuffer * buffer); + +//! get the number of bytes buffered +uint32_t bufferBufferedData(cBuffer * buffer); + +//! copy number of elements into another buffer +uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, + uint32_t size); + +//! dump (discard) the first numbytes from the front of the buffer +void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes); + +//! 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 +uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index); + +//! add a byte to the end of the buffer +uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data); + +//! queue a block of character to end of buffer +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); + +//! flush (clear) the contents of the buffer +void bufferFlush(cBuffer * buffer); + +#endif + +/** + * @} + */