rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-119 AHRS: Converted the big data structures for the WMM calculation to be allocated on the stack, which forced a dependency on FreeRTOS :-( but keeps things on the heap. Also changed the HomeLocation object to include a Set field. When this is false, OP will try and update it whenever it gets a 3D fix. If it is saved to disk with this field as true, then it won't need to get a lock to send the magnetic flux vector to the AHRS.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1346 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-21 08:35:44 +00:00 committed by peabody124
parent 4b0a0236d8
commit bb79ba66fa
9 changed files with 122 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
flight/Libraries/WorldMagModel.c
View File

@ -32,6 +32,9 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
// I don't want this dependency, but currently using pvPortMalloc
#include "openpilot.h"
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
@ -41,8 +44,8 @@
#include "WorldMagModel.h" #include "WorldMagModel.h"
#include "WMMInternal.h" #include "WMMInternal.h"
static WMMtype_Ellipsoid Ellip; static WMMtype_Ellipsoid * Ellip;
static WMMtype_MagneticModel MagneticModel; static WMMtype_MagneticModel * MagneticModel;
/************************************************************************************** /**************************************************************************************
* Example use - very simple - only two exposed functions * Example use - very simple - only two exposed functions
@ -58,24 +61,24 @@ static WMMtype_MagneticModel MagneticModel;
int WMM_Initialize() int WMM_Initialize()
// Sets default values for WMM subroutines. // Sets default values for WMM subroutines.
// UPDATES : Ellip and MagneticModel // UPDATES : Ellip and MagneticModel
{ {
// Sets WGS-84 parameters // Sets WGS-84 parameters
Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km
Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km
Ellip.fla = 1/298.257223563; // flattening Ellip->fla = 1/298.257223563; // flattening
Ellip.eps = sqrt(1- (Ellip.b*Ellip.b)/(Ellip.a*Ellip.a )); // first eccentricity Ellip->eps = sqrt(1- (Ellip->b*Ellip->b)/(Ellip->a*Ellip->a )); // first eccentricity
Ellip.epssq = (Ellip.eps*Ellip.eps); // first eccentricity squared Ellip->epssq = (Ellip->eps*Ellip->eps); // first eccentricity squared
Ellip.re = 6371.2; // Earth's radius in km Ellip->re = 6371.2; // Earth's radius in km
// Sets Magnetic Model parameters // Sets Magnetic Model parameters
MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; MagneticModel->nMax = WMM_MAX_MODEL_DEGREES;
MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES;
MagneticModel.SecularVariationUsed = 0; MagneticModel->SecularVariationUsed = 0;
// Really, Really needs to be read from a file - out of date in 2015 at latest // Really, Really needs to be read from a file - out of date in 2015 at latest
MagneticModel.EditionDate = 5.7863328170559505e-307; MagneticModel->EditionDate = 5.7863328170559505e-307;
MagneticModel.epoch = 2010.0; MagneticModel->epoch = 2010.0;
sprintf(MagneticModel.ModelName, "WMM-2010"); sprintf(MagneticModel->ModelName, "WMM-2010");
WMM_Set_Coeff_Array(); WMM_Set_Coeff_Array();
return 0; return 0;
} }
@ -84,15 +87,20 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month,
{ {
char Error_Message[255]; char Error_Message[255];
Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid));
MagneticModel = (WMMtype_MagneticModel *) pvPortMalloc(sizeof(WMMtype_MagneticModel));
WMMtype_CoordSpherical CoordSpherical; WMMtype_CoordSpherical CoordSpherical;
WMMtype_CoordGeodetic CoordGeodetic; WMMtype_CoordGeodetic CoordGeodetic;
WMMtype_Date Date; WMMtype_Date Date;
WMMtype_GeoMagneticElements GeoMagneticElements; WMMtype_GeoMagneticElements GeoMagneticElements;
WMM_Initialize();
CoordGeodetic.lambda = Lon; CoordGeodetic.lambda = Lon;
CoordGeodetic.phi = Lat; CoordGeodetic.phi = Lat;
CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid;
WMM_GeodeticToSpherical(Ellip, CoordGeodetic, &CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/ WMM_GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/
Date.Month=Month; Date.Month=Month;
Date.Day=Day; Date.Day=Day;
@ -101,6 +109,9 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month,
WMM_TimelyModifyMagneticModel(Date); WMM_TimelyModifyMagneticModel(Date);
WMM_Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/ WMM_Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/
vPortFree(Ellip);
vPortFree(MagneticModel);
B[0]=GeoMagneticElements.X; B[0]=GeoMagneticElements.X;
B[1]=GeoMagneticElements.Y; B[1]=GeoMagneticElements.Y;
B[2]=GeoMagneticElements.Z; B[2]=GeoMagneticElements.Z;
@ -136,8 +147,8 @@ uint16_t WMM_Geomag( WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeode
WMMtype_SphericalHarmonicVariables SphVariables; WMMtype_SphericalHarmonicVariables SphVariables;
WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar; WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar;
WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel.nMax, &SphVariables); /* Compute Spherical Harmonic variables */ WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */
WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel.nMax, &LegendreFunction); /* Compute ALF */ WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */
WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/
WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation 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, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */
@ -180,10 +191,10 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSphe
sin_lambda = sin(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 = 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). */ 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++) 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);
} }
/* /*
@ -270,7 +281,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
MagneticResults->Bz = 0.0; MagneticResults->Bz = 0.0;
MagneticResults->By = 0.0; MagneticResults->By = 0.0;
MagneticResults->Bx = 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++) for (m=0;m<=n;m++)
{ {
@ -281,8 +292,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/
MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] +
MagneticModel.Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] )
* (float) (n+1) * LegendreFunction-> Pcup[index]; * (float) (n+1) * LegendreFunction-> Pcup[index];
/* 1 nMax (n+2) n m m m /* 1 nMax (n+2) n m m m
@ -290,8 +301,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * MagneticResults->By += SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] -
MagneticModel.Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] ) MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] )
* (float) (m) * LegendreFunction-> Pcup[index]; * (float) (m) * LegendreFunction-> Pcup[index];
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
@ -299,8 +310,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */
MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] +
MagneticModel.Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] )
* LegendreFunction-> dPcup[index]; * LegendreFunction-> dPcup[index];
@ -340,11 +351,11 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
*/ */
uint16_t m, n, index; uint16_t m, n, index;
float cos_phi; float cos_phi;
MagneticModel.SecularVariationUsed = TRUE; MagneticModel->SecularVariationUsed = TRUE;
MagneticResults->Bz = 0.0; MagneticResults->Bz = 0.0;
MagneticResults->By = 0.0; MagneticResults->By = 0.0;
MagneticResults->Bx = 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++) for (m=0;m<=n;m++)
{ {
@ -355,8 +366,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to radius.*/ /* Derivative with respect to radius.*/
MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] +
MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] )
* (float) (n+1) * LegendreFunction-> Pcup[index]; * (float) (n+1) * LegendreFunction-> Pcup[index];
/* 1 nMax (n+2) n m m m /* 1 nMax (n+2) n m m m
@ -364,8 +375,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to longitude, divided by radius. */ /* Derivative with respect to longitude, divided by radius. */
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * MagneticResults->By += SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] -
MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] ) MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] )
* (float) (m) * LegendreFunction-> Pcup[index]; * (float) (m) * LegendreFunction-> Pcup[index];
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
@ -373,8 +384,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
/* Derivative with respect to latitude, divided by radius. */ /* Derivative with respect to latitude, divided by radius. */
MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] +
MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] )
* LegendreFunction-> dPcup[index]; * LegendreFunction-> dPcup[index];
} }
} }
@ -762,7 +773,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
MagneticResults->By = 0.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 /*Compute the ration between the Gauss-normalized associated Legendre
@ -789,8 +800,8 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * MagneticResults->By += SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] - ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] -
MagneticModel.Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] ) MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] )
* PcupS[n] * schmidtQuasiNorm3; * PcupS[n] * schmidtQuasiNorm3;
} }
@ -819,7 +830,7 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari
MagneticResults->By = 0.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); index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n;
@ -841,8 +852,8 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari
/* Derivative with respect to longitude, divided by radius. */ /* Derivative with respect to longitude, divided by radius. */
MagneticResults->By += SphVariables->RelativeRadiusPower[n] * MagneticResults->By += SphVariables->RelativeRadiusPower[n] *
( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] - ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] -
MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] ) MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] )
* PcupS[n] * schmidtQuasiNorm3; * PcupS[n] * schmidtQuasiNorm3;
} }
@ -857,17 +868,17 @@ void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate)
{ {
uint16_t n, m, index, a, b; uint16_t n, m, index, a, b;
a = MagneticModel.nMaxSecVar; a = MagneticModel->nMaxSecVar;
b = (a * (a + 1) / 2 + a); b = (a * (a + 1) / 2 + a);
for (n = 1; n <= MagneticModel.nMax; n++) for (n = 1; n <= MagneticModel->nMax; n++)
{ {
for (m=0;m<=n;m++) for (m=0;m<=n;m++)
{ {
index = (n * (n + 1) / 2 + m); index = (n * (n + 1) / 2 + m);
if(index <= b) if(index <= b)
{ {
MagneticModel.Main_Field_Coeff_H[index] += (UserDate.DecimalYear - MagneticModel.epoch) * MagneticModel.Secular_Var_Coeff_H[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]; MagneticModel->Main_Field_Coeff_G[index] += (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index];
} }
} }
} }
@ -906,7 +917,7 @@ uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error)
return 1; return 1;
} /*WMM_DateToYear*/ } /*WMM_DateToYear*/
void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical)
// Converts Geodetic coordinates to Spherical coordinates // Converts Geodetic coordinates to Spherical coordinates
// Convert geodetic coordinates, (defined by the WGS-84 // Convert geodetic coordinates, (defined by the WGS-84
// reference ellipsoid), to Earth Centered Earth Fixed Cartesian // reference ellipsoid), to Earth Centered Earth Fixed Cartesian
@ -914,22 +925,22 @@ void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic Coor
{ {
float CosLat, SinLat, rc, xp, zp; // all local variables float CosLat, SinLat, rc, xp, zp; // all local variables
CosLat = cos(DEG2RAD(CoordGeodetic.phi)); CosLat = cos(DEG2RAD(CoordGeodetic->phi));
SinLat = sin(DEG2RAD(CoordGeodetic.phi)); SinLat = sin(DEG2RAD(CoordGeodetic->phi));
// compute the local radius of curvature on the WGS-84 reference ellipsoid // compute the local radius of curvature on the WGS-84 reference ellipsoid
rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat);
// compute ECEF Cartesian coordinates of specified point (for longitude=0) // compute ECEF Cartesian coordinates of specified point (for longitude=0)
xp = (rc + CoordGeodetic.HeightAboveEllipsoid) * CosLat; xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
zp = (rc*(1.0 - Ellip.epssq) + CoordGeodetic.HeightAboveEllipsoid) * SinLat; zp = (rc*(1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
// compute spherical radius and angle lambda and phi of specified point // compute spherical radius and angle lambda and phi of specified point
CoordSpherical->r = sqrt(xp * xp + zp * zp); CoordSpherical->r = sqrt(xp * xp + zp * zp);
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude
CoordSpherical->lambda = CoordGeodetic.lambda; // longitude CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
}// WMM_GeodeticToSpherical }// WMM_GeodeticToSpherical
@ -1031,10 +1042,10 @@ void WMM_Set_Coeff_Array()
// TODO: If this works here, delete first two columns to save space // TODO: If this works here, delete first two columns to save space
for(uint16_t i=0; i<NUMTERMS; i++){ for(uint16_t i=0; i<NUMTERMS; i++){
MagneticModel.Main_Field_Coeff_G[i]=CoeffFile[i][2]; MagneticModel->Main_Field_Coeff_G[i]=CoeffFile[i][2];
MagneticModel.Main_Field_Coeff_H[i]=CoeffFile[i][3]; MagneticModel->Main_Field_Coeff_H[i]=CoeffFile[i][3];
MagneticModel.Secular_Var_Coeff_G[i]=CoeffFile[i][4]; MagneticModel->Secular_Var_Coeff_G[i]=CoeffFile[i][4];
MagneticModel.Secular_Var_Coeff_H[i]=CoeffFile[i][5]; MagneticModel->Secular_Var_Coeff_H[i]=CoeffFile[i][5];
} }
} }

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

@ -120,7 +120,7 @@
// Internal Function Prototypes // Internal Function Prototypes
void WMM_Set_Coeff_Array(); void WMM_Set_Coeff_Array();
void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical);
uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error); uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error);
void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate); void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate);
uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,

21
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -79,7 +79,8 @@ void nmeaProcessGPGSA(char* packet);
// Global variables // Global variables
// Private constants // Private constants
#define STACK_SIZE configMINIMAL_STACK_SIZE+5000 // Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE configMINIMAL_STACK_SIZE+3500
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
// Private types // Private types
@ -120,8 +121,6 @@ int32_t GPSInitialize(void)
return 0; return 0;
} }
static bool homeLocationSet = 0;
/** /**
* gps task. Processes input buffer. It does not return. * gps task. Processes input buffer. It does not return.
*/ */
@ -132,8 +131,6 @@ static void gpsTask(void* parameters)
portTickType xDelay = 100 / portTICK_RATE_MS; portTickType xDelay = 100 / portTICK_RATE_MS;
PositionActualData GpsData; PositionActualData GpsData;
uint32_t timeNowMs; uint32_t timeNowMs;
homeLocationSet = 0;
// Loop forever // Loop forever
while(1) while(1)
@ -161,13 +158,14 @@ static void gpsTask(void* parameters)
} }
else { else {
// Had an update // Had an update
HomeLocationData home;
HomeLocationGet(&home);
PositionActualGet(&GpsData); PositionActualGet(&GpsData);
if(homeLocationSet == FALSE) { if( (GpsData.Status == POSITIONACTUAL_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE) ) {
setHomeLocation(&GpsData); setHomeLocation(&GpsData);
homeLocationSet = TRUE;
} }
} }
setHomeLocation(&GpsData);
// Block task until next update // Block task until next update
vTaskDelay(xDelay); vTaskDelay(xDelay);
@ -179,10 +177,6 @@ static void setHomeLocation(PositionActualData * gpsData)
HomeLocationData home; HomeLocationData home;
HomeLocationGet(&home); HomeLocationGet(&home);
gpsData->Latitude = -29;
gpsData->Longitude = 93;
gpsData->Altitude = 30;
// Store LLA // Store LLA
home.Latitude = (int32_t) (gpsData->Latitude * 10e6); home.Latitude = (int32_t) (gpsData->Latitude * 10e6);
home.Longitude = (int32_t) (gpsData->Longitude * 10e6); home.Longitude = (int32_t) (gpsData->Longitude * 10e6);
@ -203,10 +197,9 @@ static void setHomeLocation(PositionActualData * gpsData)
memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0])); memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0]));
// Compute magnetic flux direction at home location // Compute magnetic flux direction at home location
WMM_Initialize();
// TODO: Extract time/date from GPS to seed this
WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]); WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]);
home.Set = HOMELOCATION_SET_TRUE;
HomeLocationSet(&home); HomeLocationSet(&home);
} }

6
flight/OpenPilot/UAVObjects/inc/homelocation.h
View File

@ -33,7 +33,7 @@
#define HOMELOCATION_H #define HOMELOCATION_H
// Object constants // Object constants
#define HOMELOCATION_OBJID 933172238U #define HOMELOCATION_OBJID 576787928U
#define HOMELOCATION_NAME "HomeLocation" #define HOMELOCATION_NAME "HomeLocation"
#define HOMELOCATION_METANAME "HomeLocationMeta" #define HOMELOCATION_METANAME "HomeLocationMeta"
#define HOMELOCATION_ISSINGLEINST 1 #define HOMELOCATION_ISSINGLEINST 1
@ -57,6 +57,7 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t Set;
int32_t Latitude; int32_t Latitude;
int32_t Longitude; int32_t Longitude;
float Altitude; float Altitude;
@ -67,6 +68,9 @@ typedef struct {
} __attribute__((packed)) HomeLocationData; } __attribute__((packed)) HomeLocationData;
// Field information // Field information
// Field Set information
/* Enumeration options for field Set */
typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions;
// Field Latitude information // Field Latitude information
// Field Longitude information // Field Longitude information
// Field Altitude information // Field Altitude information

6
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -22,6 +22,7 @@
651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = "<group>"; }; 651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = "<group>"; };
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; }; 654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; }; 6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = "<group>"; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = "<group>"; };
657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = "<group>"; }; 657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = "<group>"; };
657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = "<group>"; }; 657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = "<group>"; };
@ -32,6 +33,7 @@
657CEEC1121DC054007A1FBE /* homelocation.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = homelocation.c; sourceTree = "<group>"; }; 657CEEC1121DC054007A1FBE /* homelocation.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = homelocation.c; sourceTree = "<group>"; };
657CEEC2121DC054007A1FBE /* navigationdesired.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationdesired.c; sourceTree = "<group>"; }; 657CEEC2121DC054007A1FBE /* navigationdesired.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationdesired.c; sourceTree = "<group>"; };
657CEEC3121DC054007A1FBE /* navigationsettings.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationsettings.c; sourceTree = "<group>"; }; 657CEEC3121DC054007A1FBE /* navigationsettings.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationsettings.c; sourceTree = "<group>"; };
657CF024121F49CD007A1FBE /* WMMInternal.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WMMInternal.h; sourceTree = "<group>"; };
65B35D7F121C261E003EAD18 /* bin.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = bin.pro; sourceTree = "<group>"; }; 65B35D7F121C261E003EAD18 /* bin.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = bin.pro; sourceTree = "<group>"; };
65B35D80121C261E003EAD18 /* openpilotgcs */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = openpilotgcs; sourceTree = "<group>"; }; 65B35D80121C261E003EAD18 /* openpilotgcs */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = openpilotgcs; sourceTree = "<group>"; };
65B35D81121C261E003EAD18 /* openpilotgcs.pri */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = openpilotgcs.pri; sourceTree = "<group>"; }; 65B35D81121C261E003EAD18 /* openpilotgcs.pri */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = openpilotgcs.pri; sourceTree = "<group>"; };
@ -2890,8 +2892,8 @@
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
65B7E6AC120DF1CD000C1123 /* AHRS */, 65B7E6AC120DF1CD000C1123 /* AHRS */,
657CEEB6121DBC63007A1FBE /* Libraries */,
65E8EF1E11EEA61E00BBF654 /* OpenPilot */, 65E8EF1E11EEA61E00BBF654 /* OpenPilot */,
657CEEB6121DBC63007A1FBE /* Libraries */,
65E8F02F11EFF25C00BBF654 /* PiOS */, 65E8F02F11EFF25C00BBF654 /* PiOS */,
C6A0FF2B0290797F04C91782 /* Documentation */, C6A0FF2B0290797F04C91782 /* Documentation */,
1AB674ADFE9D54B511CA2CBB /* Products */, 1AB674ADFE9D54B511CA2CBB /* Products */,
@ -2913,6 +2915,7 @@
657CEEB8121DBC63007A1FBE /* inc */ = { 657CEEB8121DBC63007A1FBE /* inc */ = {
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
657CF024121F49CD007A1FBE /* WMMInternal.h */,
657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */, 657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */,
657CEEBA121DBC63007A1FBE /* WorldMagModel.h */, 657CEEBA121DBC63007A1FBE /* WorldMagModel.h */,
); );
@ -6664,6 +6667,7 @@
65B367E3121C2620003EAD18 /* uavobjectdefinition */ = { 65B367E3121C2620003EAD18 /* uavobjectdefinition */ = {
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
655268BC121FBD2900410C6E /* ahrscalibration.xml */,
65B367E4121C2620003EAD18 /* actuatorcommand.xml */, 65B367E4121C2620003EAD18 /* actuatorcommand.xml */,
65B367E5121C2620003EAD18 /* actuatordesired.xml */, 65B367E5121C2620003EAD18 /* actuatordesired.xml */,
65B367E6121C2620003EAD18 /* actuatorsettings.xml */, 65B367E6121C2620003EAD18 /* actuatorsettings.xml */,

6
ground/src/plugins/uavobjects/homelocation.cpp
View File

@ -42,6 +42,12 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
{ {
// Create fields // Create fields
QList<UAVObjectField*> fields; QList<UAVObjectField*> fields;
QStringList SetElemNames;
SetElemNames.append("0");
QStringList SetEnumOptions;
SetEnumOptions.append("FALSE");
SetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) );
QStringList LatitudeElemNames; QStringList LatitudeElemNames;
LatitudeElemNames.append("0"); LatitudeElemNames.append("0");
fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) );

6
ground/src/plugins/uavobjects/homelocation.h
View File

@ -43,6 +43,7 @@ class UAVOBJECTS_EXPORT HomeLocation: public UAVDataObject
public: public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 Set;
qint32 Latitude; qint32 Latitude;
qint32 Longitude; qint32 Longitude;
float Altitude; float Altitude;
@ -53,6 +54,9 @@ public:
} __attribute__((packed)) DataFields; } __attribute__((packed)) DataFields;
// Field information // Field information
// Field Set information
/* Enumeration options for field Set */
typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions;
// Field Latitude information // Field Latitude information
// Field Longitude information // Field Longitude information
// Field Altitude information // Field Altitude information
@ -68,7 +72,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 933172238U; static const quint32 OBJID = 576787928U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0; static const bool ISSETTINGS = 0;

14
ground/src/plugins/uavobjects/homelocation.py
View File

@ -37,6 +37,18 @@ from collections import namedtuple
# This is a list of instances of the data fields contained in this object # This is a list of instances of the data fields contained in this object
_fields = [ \ _fields = [ \
uavobject.UAVObjectField(
'Set',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'Latitude', 'Latitude',
'i', 'i',
@ -114,7 +126,7 @@ _fields = [ \
class HomeLocation(uavobject.UAVObject): class HomeLocation(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 933172238 OBJID = 576787928
NAME = "HomeLocation" NAME = "HomeLocation"
METANAME = "HomeLocationMeta" METANAME = "HomeLocationMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

25
ground/src/shared/uavobjectdefinition/homelocation.xml
View File

@ -1,14 +1,15 @@
<xml> <xml>
<object name="HomeLocation" singleinstance="true" settings="false"> <object name="HomeLocation" singleinstance="true" settings="false">
<field name="Latitude" units="deg * 10e6" type="int32" elements="1"/> <field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="Longitude" units="deg * 10e6" type="int32" elements="1"/> <field name="Latitude" units="deg * 10e6" type="int32" elements="1"/>
<field name="Altitude" units="m over geoid" type="float" elements="1"/> <field name="Longitude" units="deg * 10e6" type="int32" elements="1"/>
<field name="ECEF" units="m" type="float" elements="3"/> <field name="Altitude" units="m over geoid" type="float" elements="1"/>
<field name="RNE" units="" type="float" elements="9"/> <field name="ECEF" units="m" type="float" elements="3"/>
<field name="Be" units="" type="float" elements="3"/> <field name="RNE" units="" type="float" elements="9"/>
<access gcs="readwrite" flight="readwrite"/> <field name="Be" units="" type="float" elements="3"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <access gcs="readwrite" flight="readwrite"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<logging updatemode="never" period="0"/> <telemetryflight acked="false" updatemode="periodic" period="10000"/>
</object> <logging updatemode="never" period="0"/>
</object>
</xml> </xml>