diff --git a/flight/Libraries/WorldMagModel.c b/flight/Libraries/WorldMagModel.c index f4afb08f6..6b5b71112 100644 --- a/flight/Libraries/WorldMagModel.c +++ b/flight/Libraries/WorldMagModel.c @@ -41,8 +41,8 @@ #include "WorldMagModel.h" #include "WMMInternal.h" -WMMtype_Ellipsoid Ellip; -WMMtype_MagneticModel MagneticModel; +static WMMtype_Ellipsoid Ellip; +static WMMtype_MagneticModel MagneticModel; /************************************************************************************** * Example use - very simple - only two exposed functions @@ -55,13 +55,11 @@ WMMtype_MagneticModel MagneticModel; * B is the NED (XYZ) magnetic vector in nTesla **************************************************************************************/ -void WMM_Initialize() +int WMM_Initialize() // Sets default values for WMM subroutines. // UPDATES : Ellip and MagneticModel { - float coeffs[NUMTERMS][6]; - - // Sets WGS-84 parameters + // 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 @@ -78,20 +76,14 @@ void WMM_Initialize() MagneticModel.EditionDate = 5.7863328170559505e-307; MagneticModel.epoch = 2010.0; sprintf(MagneticModel.ModelName, "WMM-2010"); - WMM_Set_Coeff_Array(coeffs); - for(uint16_t i=0; inMax, &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_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; + WMM_CalculateSecularVariation(&MagneticResultsGeoVar, GeoMagneticElements); /*Calculate the secular variation of each of the Geomagnetic elements*/ + return TRUE; } -uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_Ellipsoid Ellip, WMMtype_CoordSpherical CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables *SphVariables) +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 @@ -186,14 +176,14 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_Ellipsoid Ellip, WMMtype { float cos_lambda, sin_lambda; uint16_t m, n; - cos_lambda = cos(DEG2RAD(CoordSpherical.lambda)); - sin_lambda = sin(DEG2RAD(CoordSpherical.lambda)); + 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); + 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); + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n-1] * (Ellip.re / CoordSpherical->r); } /* @@ -215,7 +205,7 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_Ellipsoid Ellip, WMMtype } /*WMM_ComputeSphericalHarmonicVariables*/ -uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical CoordSpherical, uint16_t nMax, WMMtype_LegendreFunction *LegendreFunction) +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. @@ -237,7 +227,7 @@ uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical CoordSpherical, u float sin_phi; uint16_t FLAG = 1; - sin_phi = sin ( DEG2RAD ( CoordSpherical.phig ) ); /* sin (geocentric latitude) */ + 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); @@ -249,7 +239,7 @@ uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical CoordSpherical, u return TRUE; } /*WMM_AssociatedLegendreFunction */ -uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_MagneticModel *MagneticModel, 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) { /* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using spherical harmonic summation. @@ -280,7 +270,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Magne MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel->nMax; n++) + for (n = 1; n <= MagneticModel.nMax; n++) { for (m=0;m<=n;m++) { @@ -290,27 +280,27 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Magne 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] ) + 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] ) + 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] ) + 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]; @@ -318,7 +308,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Magne } } - cos_phi = cos ( DEG2RAD ( CoordSpherical.phig ) ); + cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); if ( fabs(cos_phi) > 1.0e-10 ) { MagneticResults->By = MagneticResults->By / cos_phi ; @@ -331,12 +321,12 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Magne */ { - WMM_SummationSpecial(MagneticModel, SphVariables, CoordSpherical, MagneticResults); + WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults); } return TRUE; }/*WMM_Summation */ -uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_MagneticModel *MagneticModel, WMMtype_SphericalHarmonicVariables SphVariables, WMMtype_CoordSpherical CoordSpherical, WMMtype_MagneticResults *MagneticResults) +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 @@ -350,11 +340,11 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype */ uint16_t m, n, index; float cos_phi; - MagneticModel->SecularVariationUsed = TRUE; + MagneticModel.SecularVariationUsed = TRUE; MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) + for (n = 1; n <= MagneticModel.nMaxSecVar; n++) { for (m=0;m<=n;m++) { @@ -364,31 +354,31 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype 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] ) + 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] ) + 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] ) + 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 ) ); + cos_phi = cos ( DEG2RAD ( CoordSpherical->phig ) ); if ( fabs(cos_phi) > 1.0e-10 ) { MagneticResults->By = MagneticResults->By / cos_phi ; @@ -396,12 +386,12 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype else /* Special calculation for component By at Geographic poles */ { - WMM_SecVarSummationSpecial(MagneticModel, SphVariables, CoordSpherical, MagneticResults); + 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) +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 @@ -433,12 +423,12 @@ uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical CoordSpherical, WMMtype { float Psi; /* Difference between the spherical and Geodetic latitudes */ - Psi = ( M_PI/180 ) * ( CoordSpherical.phig - CoordGeodetic.phi ); + 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; + 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*/ @@ -472,7 +462,7 @@ uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *MagneticResul return TRUE; } /*WMM_CalculateGeoMagneticElements */ -uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements) +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 ) @@ -491,9 +481,9 @@ uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults MagneticVariation */ { - MagneticElements->Xdot = MagneticVariation.Bx; - MagneticElements->Ydot = MagneticVariation.By; - MagneticElements->Zdot = MagneticVariation.Bz; + 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); @@ -751,7 +741,7 @@ uint16_t WMM_PcupLow( float *Pcup, float *dPcup, float x, uint16_t nMax) } /*WMM_PcupLow */ -uint16_t WMM_SummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtype_SphericalHarmonicVariables SphVariables, WMMtype_CoordSpherical CoordSpherical, WMMtype_MagneticResults *MagneticResults) +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 @@ -770,9 +760,9 @@ uint16_t WMM_SummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtype_Sphe schmidtQuasiNorm1 = 1.0; MagneticResults->By = 0.0; - sin_phi = sin ( DEG2RAD ( CoordSpherical.phig ) ); + sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - for (n = 1; n <= MagneticModel->nMax; n++) + for (n = 1; n <= MagneticModel.nMax; n++) { /*Compute the ration between the Gauss-normalized associated Legendre @@ -798,16 +788,16 @@ uint16_t WMM_SummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtype_Sphe 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] ) + 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_MagneticModel *MagneticModel, WMMtype_SphericalHarmonicVariables SphVariables, WMMtype_CoordSpherical CoordSpherical, WMMtype_MagneticResults *MagneticResults) +uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults *MagneticResults) { /*Special calculation for the secular variation summation at the poles. @@ -827,9 +817,9 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtyp schmidtQuasiNorm1 = 1.0; MagneticResults->By = 0.0; - sin_phi = sin ( DEG2RAD ( CoordSpherical.phig ) ); + sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - for (n = 1; n <= MagneticModel->nMaxSecVar; n++) + for (n = 1; n <= MagneticModel.nMaxSecVar; n++) { index = (n * (n + 1) / 2 + 1); schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; @@ -850,9 +840,9 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtyp 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] ) + 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; } @@ -860,38 +850,24 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_MagneticModel *MagneticModel, WMMtyp }/*SecVarSummationSpecial*/ -void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate, WMMtype_MagneticModel *MagneticModel, WMMtype_MagneticModel *TimedMagneticModel) +void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate) // Time change the Model coefficients from the base year of the model using secular variation coefficients. -// Store the coefficients of the static model with their values advanced from epoch t0 to epoch t. -// Copy the SV coefficients. If input "t" is the same as "t0", then this is merely a copy operation. -// If the address of "TimedMagneticModel" is the same as the address of "MagneticModel", then this procedure overwrites -// the given item "MagneticModel". +// +// Modified to work on the global data structure to reduce memory footprint { uint16_t n, m, index, a, b; - TimedMagneticModel->EditionDate = MagneticModel->EditionDate; - TimedMagneticModel->epoch = MagneticModel->epoch; - TimedMagneticModel->nMax = MagneticModel->nMax; - TimedMagneticModel->nMaxSecVar = MagneticModel->nMaxSecVar; - a = TimedMagneticModel->nMaxSecVar; + a = MagneticModel.nMaxSecVar; b = (a * (a + 1) / 2 + a); - strcpy(TimedMagneticModel->ModelName,MagneticModel->ModelName); - for (n = 1; n <= MagneticModel->nMax; n++) + for (n = 1; n <= MagneticModel.nMax; n++) { for (m=0;m<=n;m++) { index = (n * (n + 1) / 2 + m); if(index <= b) { - TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index] + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; - TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index] + (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; - TimedMagneticModel->Secular_Var_Coeff_H[index] = MagneticModel->Secular_Var_Coeff_H[index]; // We need a copy of the secular var coef to calculate secular change - TimedMagneticModel->Secular_Var_Coeff_G[index] = MagneticModel->Secular_Var_Coeff_G[index]; - } - else - { - TimedMagneticModel->Main_Field_Coeff_H[index] = MagneticModel->Main_Field_Coeff_H[index]; - TimedMagneticModel->Main_Field_Coeff_G[index] = MagneticModel->Main_Field_Coeff_G[index]; + 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]; } } } @@ -957,9 +933,10 @@ void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic Coor }// WMM_GeodeticToSpherical -void WMM_Set_Coeff_Array(float coeffs[][6]) +void WMM_Set_Coeff_Array() { - float CoeffFile[91][6] = + // 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}, @@ -1052,9 +1029,12 @@ void WMM_Set_Coeff_Array(float coeffs[][6]) {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; iLatitude = -29; + gpsData->Longitude = 93; + gpsData->Altitude = 30; + // Store LLA home.Latitude = (int32_t) (gpsData->Latitude * 10e6); home.Longitude = (int32_t) (gpsData->Longitude * 10e6); @@ -198,13 +203,9 @@ static void setHomeLocation(PositionActualData * gpsData) memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0])); // Compute magnetic flux direction at home location - //WMM_Initialize(); // Set default values and constants + WMM_Initialize(); // TODO: Extract time/date from GPS to seed this - //WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, home.Be); - // Hard code flux until get this updated - will get wrong orientations - home.Be[0] = 1; - home.Be[1] = 0; - home.Be[2] = 0; + WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]); HomeLocationSet(&home); }