mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
OP-119 AHRS: Increased GPS stack size to allow WWM to not overwrite other memory. In addition converted all the calls to use pointers so I will switch to using pvPortMalloc vPortFree to get it off the stack entirely. Now correctly computing flux for me and running.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1345 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
2031cfe4d0
commit
4b0a0236d8
@ -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; i<NUMTERMS; i++){
|
||||
MagneticModel.Main_Field_Coeff_G[i]=coeffs[i][2];
|
||||
MagneticModel.Main_Field_Coeff_H[i]=coeffs[i][3];
|
||||
MagneticModel.Secular_Var_Coeff_G[i]=coeffs[i][4];
|
||||
MagneticModel.Secular_Var_Coeff_H[i]=coeffs[i][5];
|
||||
}
|
||||
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];
|
||||
|
||||
WMMtype_MagneticModel TimedMagneticModel;
|
||||
WMMtype_CoordSpherical CoordSpherical;
|
||||
WMMtype_CoordGeodetic CoordGeodetic;
|
||||
WMMtype_Date Date;
|
||||
@ -105,17 +97,17 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month,
|
||||
Date.Month=Month;
|
||||
Date.Day=Day;
|
||||
Date.Year=Year;
|
||||
WMM_DateToYear (&Date, Error_Message);
|
||||
WMM_TimelyModifyMagneticModel(Date, &MagneticModel, &TimedMagneticModel); /* Time adjust the coefficients, Equation 19, WMM Technical report */
|
||||
WMM_Geomag(Ellip, CoordSpherical, CoordGeodetic, &TimedMagneticModel, &GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/
|
||||
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;
|
||||
}
|
||||
|
||||
uint16_t WMM_Geomag(WMMtype_Ellipsoid Ellip, WMMtype_CoordSpherical CoordSpherical, WMMtype_CoordGeodetic CoordGeodetic,
|
||||
WMMtype_MagneticModel *TimedMagneticModel, WMMtype_GeoMagneticElements *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
|
||||
@ -140,25 +132,23 @@ uint16_t WMM_Geomag(WMMtype_Ellipsoid Ellip, WMMtype_CoordSpherical CoordSpheri
|
||||
|
||||
*/
|
||||
{
|
||||
WMMtype_LegendreFunction LegendreAllocate, *LegendreFunction;
|
||||
LegendreFunction = &LegendreAllocate;
|
||||
WMMtype_LegendreFunction LegendreFunction;
|
||||
WMMtype_SphericalHarmonicVariables SphVariables;
|
||||
WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar;
|
||||
|
||||
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_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; i<NUMTERMS; i++){
|
||||
for(uint16_t j=0; j<6; j++)
|
||||
coeffs[i][j]=CoeffFile[i][j];
|
||||
}
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -119,25 +119,21 @@
|
||||
} WMMtype_GeoMagneticElements;
|
||||
|
||||
// Internal Function Prototypes
|
||||
void WMM_Set_Coeff_Array(float coeffs[][6]);
|
||||
void WMM_Set_Coeff_Array();
|
||||
void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical);
|
||||
uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error);
|
||||
void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate, WMMtype_MagneticModel *MagneticModel, WMMtype_MagneticModel *TimedMagneticModel);
|
||||
|
||||
uint16_t WMM_Geomag(WMMtype_Ellipsoid Ellip,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
WMMtype_CoordGeodetic CoordGeodetic,
|
||||
WMMtype_MagneticModel *TimedMagneticModel,
|
||||
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_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_CalculateSecularVariation(WMMtype_MagneticResults *MagneticVariation, WMMtype_GeoMagneticElements *MagneticElements);
|
||||
|
||||
uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_Ellipsoid Ellip,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
uint16_t WMM_ComputeSphericalHarmonicVariables( WMMtype_CoordSpherical *CoordSpherical,
|
||||
uint16_t nMax,
|
||||
WMMtype_SphericalHarmonicVariables * SphVariables);
|
||||
|
||||
@ -145,31 +141,27 @@
|
||||
|
||||
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,
|
||||
uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * ,
|
||||
WMMtype_CoordGeodetic * CoordGeodetic,
|
||||
WMMtype_MagneticResults * MagneticResultsSph,
|
||||
WMMtype_MagneticResults *MagneticResultsGeo);
|
||||
|
||||
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction,
|
||||
WMMtype_MagneticModel *MagneticModel,
|
||||
WMMtype_SphericalHarmonicVariables SphVariables,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
WMMtype_SphericalHarmonicVariables * SphVariables,
|
||||
WMMtype_CoordSpherical * CoordSpherical,
|
||||
WMMtype_MagneticResults *MagneticResults);
|
||||
|
||||
uint16_t WMM_SecVarSummationSpecial(WMMtype_MagneticModel *MagneticModel,
|
||||
WMMtype_SphericalHarmonicVariables SphVariables,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
|
||||
WMMtype_CoordSpherical * CoordSpherical,
|
||||
WMMtype_MagneticResults *MagneticResults);
|
||||
|
||||
uint16_t WMM_Summation( WMMtype_LegendreFunction *LegendreFunction,
|
||||
WMMtype_MagneticModel *MagneticModel,
|
||||
WMMtype_SphericalHarmonicVariables SphVariables,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
WMMtype_SphericalHarmonicVariables * SphVariables,
|
||||
WMMtype_CoordSpherical * CoordSpherical,
|
||||
WMMtype_MagneticResults *MagneticResults);
|
||||
|
||||
uint16_t WMM_SummationSpecial(WMMtype_MagneticModel *MagneticModel,
|
||||
WMMtype_SphericalHarmonicVariables SphVariables,
|
||||
WMMtype_CoordSpherical CoordSpherical,
|
||||
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
|
||||
WMMtype_CoordSpherical * CoordSpherical,
|
||||
WMMtype_MagneticResults *MagneticResults);
|
||||
|
||||
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
#define WORLDMAGMODEL_H_
|
||||
|
||||
// Exposed Function Prototypes
|
||||
void WMM_Initialize();
|
||||
int WMM_Initialize();
|
||||
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]);
|
||||
|
||||
#endif /* WORLDMAGMODEL_H_ */
|
||||
@ -79,7 +79,7 @@ void nmeaProcessGPGSA(char* packet);
|
||||
// Global variables
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE+5000
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
|
||||
// Private types
|
||||
@ -162,11 +162,12 @@ static void gpsTask(void* parameters)
|
||||
else {
|
||||
// Had an update
|
||||
PositionActualGet(&GpsData);
|
||||
if(GpsData.Status == POSITIONACTUAL_STATUS_FIX3D && homeLocationSet == FALSE) {
|
||||
if(homeLocationSet == FALSE) {
|
||||
setHomeLocation(&GpsData);
|
||||
homeLocationSet = TRUE;
|
||||
}
|
||||
}
|
||||
setHomeLocation(&GpsData);
|
||||
|
||||
// Block task until next update
|
||||
vTaskDelay(xDelay);
|
||||
@ -178,6 +179,10 @@ static void setHomeLocation(PositionActualData * gpsData)
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
gpsData->Latitude = -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);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user