mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-26 15:54:15 +01:00
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.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1346 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
4b0a0236d8
commit
bb79ba66fa
@ -32,6 +32,9 @@
|
||||
* 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 <string.h>
|
||||
#include <math.h>
|
||||
@ -41,8 +44,8 @@
|
||||
#include "WorldMagModel.h"
|
||||
#include "WMMInternal.h"
|
||||
|
||||
static WMMtype_Ellipsoid Ellip;
|
||||
static WMMtype_MagneticModel MagneticModel;
|
||||
static WMMtype_Ellipsoid * Ellip;
|
||||
static WMMtype_MagneticModel * MagneticModel;
|
||||
|
||||
/**************************************************************************************
|
||||
* Example use - very simple - only two exposed functions
|
||||
@ -58,24 +61,24 @@ static WMMtype_MagneticModel MagneticModel;
|
||||
int WMM_Initialize()
|
||||
// Sets default values for WMM subroutines.
|
||||
// UPDATES : Ellip and MagneticModel
|
||||
{
|
||||
{
|
||||
// Sets WGS-84 parameters
|
||||
Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km
|
||||
Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km
|
||||
Ellip.fla = 1/298.257223563; // flattening
|
||||
Ellip.eps = sqrt(1- (Ellip.b*Ellip.b)/(Ellip.a*Ellip.a )); // first eccentricity
|
||||
Ellip.epssq = (Ellip.eps*Ellip.eps); // first eccentricity squared
|
||||
Ellip.re = 6371.2; // Earth's radius in km
|
||||
Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km
|
||||
Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km
|
||||
Ellip->fla = 1/298.257223563; // flattening
|
||||
Ellip->eps = sqrt(1- (Ellip->b*Ellip->b)/(Ellip->a*Ellip->a )); // first eccentricity
|
||||
Ellip->epssq = (Ellip->eps*Ellip->eps); // first eccentricity squared
|
||||
Ellip->re = 6371.2; // Earth's radius in km
|
||||
|
||||
// Sets Magnetic Model parameters
|
||||
MagneticModel.nMax = WMM_MAX_MODEL_DEGREES;
|
||||
MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES;
|
||||
MagneticModel.SecularVariationUsed = 0;
|
||||
MagneticModel->nMax = WMM_MAX_MODEL_DEGREES;
|
||||
MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES;
|
||||
MagneticModel->SecularVariationUsed = 0;
|
||||
|
||||
// Really, Really needs to be read from a file - out of date in 2015 at latest
|
||||
MagneticModel.EditionDate = 5.7863328170559505e-307;
|
||||
MagneticModel.epoch = 2010.0;
|
||||
sprintf(MagneticModel.ModelName, "WMM-2010");
|
||||
MagneticModel->EditionDate = 5.7863328170559505e-307;
|
||||
MagneticModel->epoch = 2010.0;
|
||||
sprintf(MagneticModel->ModelName, "WMM-2010");
|
||||
WMM_Set_Coeff_Array();
|
||||
return 0;
|
||||
}
|
||||
@ -84,15 +87,20 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month,
|
||||
{
|
||||
char Error_Message[255];
|
||||
|
||||
Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid));
|
||||
MagneticModel = (WMMtype_MagneticModel *) pvPortMalloc(sizeof(WMMtype_MagneticModel));
|
||||
|
||||
WMMtype_CoordSpherical CoordSpherical;
|
||||
WMMtype_CoordGeodetic CoordGeodetic;
|
||||
WMMtype_Date Date;
|
||||
WMMtype_GeoMagneticElements GeoMagneticElements;
|
||||
|
||||
WMM_Initialize();
|
||||
|
||||
CoordGeodetic.lambda = Lon;
|
||||
CoordGeodetic.phi = Lat;
|
||||
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.Day=Day;
|
||||
@ -101,6 +109,9 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month,
|
||||
WMM_TimelyModifyMagneticModel(Date);
|
||||
WMM_Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/
|
||||
|
||||
vPortFree(Ellip);
|
||||
vPortFree(MagneticModel);
|
||||
|
||||
B[0]=GeoMagneticElements.X;
|
||||
B[1]=GeoMagneticElements.Y;
|
||||
B[2]=GeoMagneticElements.Z;
|
||||
@ -136,8 +147,8 @@ uint16_t WMM_Geomag( WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeode
|
||||
WMMtype_SphericalHarmonicVariables SphVariables;
|
||||
WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar;
|
||||
|
||||
WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel.nMax, &SphVariables); /* Compute Spherical Harmonic variables */
|
||||
WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel.nMax, &LegendreFunction); /* Compute ALF */
|
||||
WMM_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 */
|
||||
@ -180,10 +191,10 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSphe
|
||||
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);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -270,7 +281,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
|
||||
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++)
|
||||
{
|
||||
@ -281,8 +292,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
|
||||
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] )
|
||||
( 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
|
||||
@ -290,8 +301,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher
|
||||
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] )
|
||||
( 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))
|
||||
@ -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. */
|
||||
|
||||
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] )
|
||||
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] +
|
||||
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] )
|
||||
* LegendreFunction-> dPcup[index];
|
||||
|
||||
|
||||
@ -340,11 +351,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++)
|
||||
{
|
||||
@ -355,8 +366,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
|
||||
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] )
|
||||
( 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
|
||||
@ -364,8 +375,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
|
||||
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] )
|
||||
( 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))
|
||||
@ -373,8 +384,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype
|
||||
/* 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] )
|
||||
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] +
|
||||
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] )
|
||||
* LegendreFunction-> dPcup[index];
|
||||
}
|
||||
}
|
||||
@ -762,7 +773,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables,
|
||||
MagneticResults->By = 0.0;
|
||||
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
|
||||
@ -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. */
|
||||
|
||||
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] )
|
||||
( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] -
|
||||
MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] )
|
||||
* PcupS[n] * schmidtQuasiNorm3;
|
||||
}
|
||||
|
||||
@ -819,7 +830,7 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari
|
||||
MagneticResults->By = 0.0;
|
||||
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;
|
||||
@ -841,8 +852,8 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari
|
||||
/* 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] )
|
||||
( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] -
|
||||
MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] )
|
||||
* PcupS[n] * schmidtQuasiNorm3;
|
||||
}
|
||||
|
||||
@ -857,17 +868,17 @@ void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate)
|
||||
{
|
||||
uint16_t n, m, index, a, b;
|
||||
|
||||
a = MagneticModel.nMaxSecVar;
|
||||
a = MagneticModel->nMaxSecVar;
|
||||
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++)
|
||||
{
|
||||
index = (n * (n + 1) / 2 + m);
|
||||
if(index <= b)
|
||||
{
|
||||
MagneticModel.Main_Field_Coeff_H[index] += (UserDate.DecimalYear - MagneticModel.epoch) * MagneticModel.Secular_Var_Coeff_H[index];
|
||||
MagneticModel.Main_Field_Coeff_G[index] += (UserDate.DecimalYear - MagneticModel.epoch) * MagneticModel.Secular_Var_Coeff_G[index];
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -906,7 +917,7 @@ uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error)
|
||||
return 1;
|
||||
} /*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
|
||||
// Convert geodetic coordinates, (defined by the WGS-84
|
||||
// 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
|
||||
|
||||
CosLat = cos(DEG2RAD(CoordGeodetic.phi));
|
||||
SinLat = sin(DEG2RAD(CoordGeodetic.phi));
|
||||
CosLat = cos(DEG2RAD(CoordGeodetic->phi));
|
||||
SinLat = sin(DEG2RAD(CoordGeodetic->phi));
|
||||
|
||||
// compute the local radius of curvature on the WGS-84 reference ellipsoid
|
||||
rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat);
|
||||
rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat);
|
||||
|
||||
// compute ECEF Cartesian coordinates of specified point (for longitude=0)
|
||||
|
||||
xp = (rc + CoordGeodetic.HeightAboveEllipsoid) * CosLat;
|
||||
zp = (rc*(1.0 - Ellip.epssq) + CoordGeodetic.HeightAboveEllipsoid) * SinLat;
|
||||
xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
|
||||
zp = (rc*(1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
|
||||
|
||||
// compute spherical radius and angle lambda and phi of specified point
|
||||
|
||||
CoordSpherical->r = sqrt(xp * xp + zp * zp);
|
||||
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude
|
||||
CoordSpherical->lambda = CoordGeodetic.lambda; // longitude
|
||||
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
|
||||
|
||||
}// WMM_GeodeticToSpherical
|
||||
|
||||
@ -1031,10 +1042,10 @@ void WMM_Set_Coeff_Array()
|
||||
|
||||
// TODO: If this works here, delete first two columns to save space
|
||||
for(uint16_t i=0; i<NUMTERMS; i++){
|
||||
MagneticModel.Main_Field_Coeff_G[i]=CoeffFile[i][2];
|
||||
MagneticModel.Main_Field_Coeff_H[i]=CoeffFile[i][3];
|
||||
MagneticModel.Secular_Var_Coeff_G[i]=CoeffFile[i][4];
|
||||
MagneticModel.Secular_Var_Coeff_H[i]=CoeffFile[i][5];
|
||||
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];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -120,7 +120,7 @@
|
||||
|
||||
// Internal Function Prototypes
|
||||
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);
|
||||
void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate);
|
||||
uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
|
||||
|
||||
@ -79,7 +79,8 @@ void nmeaProcessGPGSA(char* packet);
|
||||
// Global variables
|
||||
|
||||
// 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)
|
||||
|
||||
// Private types
|
||||
@ -120,8 +121,6 @@ int32_t GPSInitialize(void)
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool homeLocationSet = 0;
|
||||
|
||||
/**
|
||||
* gps task. Processes input buffer. It does not return.
|
||||
*/
|
||||
@ -132,8 +131,6 @@ static void gpsTask(void* parameters)
|
||||
portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
PositionActualData GpsData;
|
||||
uint32_t timeNowMs;
|
||||
|
||||
homeLocationSet = 0;
|
||||
|
||||
// Loop forever
|
||||
while(1)
|
||||
@ -161,13 +158,14 @@ static void gpsTask(void* parameters)
|
||||
}
|
||||
else {
|
||||
// Had an update
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
PositionActualGet(&GpsData);
|
||||
if(homeLocationSet == FALSE) {
|
||||
if( (GpsData.Status == POSITIONACTUAL_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE) ) {
|
||||
setHomeLocation(&GpsData);
|
||||
homeLocationSet = TRUE;
|
||||
}
|
||||
}
|
||||
setHomeLocation(&GpsData);
|
||||
|
||||
// Block task until next update
|
||||
vTaskDelay(xDelay);
|
||||
@ -179,10 +177,6 @@ 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);
|
||||
@ -203,10 +197,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();
|
||||
// TODO: Extract time/date from GPS to seed this
|
||||
WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]);
|
||||
|
||||
home.Set = HOMELOCATION_SET_TRUE;
|
||||
HomeLocationSet(&home);
|
||||
}
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
#define HOMELOCATION_H
|
||||
|
||||
// Object constants
|
||||
#define HOMELOCATION_OBJID 933172238U
|
||||
#define HOMELOCATION_OBJID 576787928U
|
||||
#define HOMELOCATION_NAME "HomeLocation"
|
||||
#define HOMELOCATION_METANAME "HomeLocationMeta"
|
||||
#define HOMELOCATION_ISSINGLEINST 1
|
||||
@ -57,6 +57,7 @@
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Set;
|
||||
int32_t Latitude;
|
||||
int32_t Longitude;
|
||||
float Altitude;
|
||||
@ -67,6 +68,9 @@ typedef struct {
|
||||
} __attribute__((packed)) HomeLocationData;
|
||||
|
||||
// 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 Longitude information
|
||||
// Field Altitude information
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
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; };
|
||||
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>"; };
|
||||
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>"; };
|
||||
@ -32,6 +33,7 @@
|
||||
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>"; };
|
||||
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>"; };
|
||||
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>"; };
|
||||
@ -2890,8 +2892,8 @@
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65B7E6AC120DF1CD000C1123 /* AHRS */,
|
||||
657CEEB6121DBC63007A1FBE /* Libraries */,
|
||||
65E8EF1E11EEA61E00BBF654 /* OpenPilot */,
|
||||
657CEEB6121DBC63007A1FBE /* Libraries */,
|
||||
65E8F02F11EFF25C00BBF654 /* PiOS */,
|
||||
C6A0FF2B0290797F04C91782 /* Documentation */,
|
||||
1AB674ADFE9D54B511CA2CBB /* Products */,
|
||||
@ -2913,6 +2915,7 @@
|
||||
657CEEB8121DBC63007A1FBE /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
657CF024121F49CD007A1FBE /* WMMInternal.h */,
|
||||
657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */,
|
||||
657CEEBA121DBC63007A1FBE /* WorldMagModel.h */,
|
||||
);
|
||||
@ -6664,6 +6667,7 @@
|
||||
65B367E3121C2620003EAD18 /* uavobjectdefinition */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
655268BC121FBD2900410C6E /* ahrscalibration.xml */,
|
||||
65B367E4121C2620003EAD18 /* actuatorcommand.xml */,
|
||||
65B367E5121C2620003EAD18 /* actuatordesired.xml */,
|
||||
65B367E6121C2620003EAD18 /* actuatorsettings.xml */,
|
||||
|
||||
@ -42,6 +42,12 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
|
||||
{
|
||||
// Create 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;
|
||||
LatitudeElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) );
|
||||
|
||||
@ -43,6 +43,7 @@ class UAVOBJECTS_EXPORT HomeLocation: public UAVDataObject
|
||||
public:
|
||||
// Field structure
|
||||
typedef struct {
|
||||
quint8 Set;
|
||||
qint32 Latitude;
|
||||
qint32 Longitude;
|
||||
float Altitude;
|
||||
@ -53,6 +54,9 @@ public:
|
||||
} __attribute__((packed)) DataFields;
|
||||
|
||||
// Field information
|
||||
// Field Set information
|
||||
/* Enumeration options for field Set */
|
||||
typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions;
|
||||
// Field Latitude information
|
||||
// Field Longitude information
|
||||
// Field Altitude information
|
||||
@ -68,7 +72,7 @@ public:
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 933172238U;
|
||||
static const quint32 OBJID = 576787928U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 0;
|
||||
|
||||
@ -37,6 +37,18 @@ from collections import namedtuple
|
||||
|
||||
# This is a list of instances of the data fields contained in this object
|
||||
_fields = [ \
|
||||
uavobject.UAVObjectField(
|
||||
'Set',
|
||||
'b',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
'0' : 'FALSE',
|
||||
'1' : 'TRUE',
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'Latitude',
|
||||
'i',
|
||||
@ -114,7 +126,7 @@ _fields = [ \
|
||||
|
||||
class HomeLocation(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 933172238
|
||||
OBJID = 576787928
|
||||
NAME = "HomeLocation"
|
||||
METANAME = "HomeLocationMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
||||
@ -1,14 +1,15 @@
|
||||
<xml>
|
||||
<object name="HomeLocation" singleinstance="true" settings="false">
|
||||
<field name="Latitude" units="deg * 10e6" type="int32" elements="1"/>
|
||||
<field name="Longitude" units="deg * 10e6" type="int32" elements="1"/>
|
||||
<field name="Altitude" units="m over geoid" type="float" elements="1"/>
|
||||
<field name="ECEF" units="m" type="float" elements="3"/>
|
||||
<field name="RNE" units="" type="float" elements="9"/>
|
||||
<field name="Be" units="" type="float" elements="3"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="10000"/>
|
||||
<logging updatemode="never" period="0"/>
|
||||
</object>
|
||||
<object name="HomeLocation" singleinstance="true" settings="false">
|
||||
<field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE"/>
|
||||
<field name="Latitude" units="deg * 10e6" type="int32" elements="1"/>
|
||||
<field name="Longitude" units="deg * 10e6" type="int32" elements="1"/>
|
||||
<field name="Altitude" units="m over geoid" type="float" elements="1"/>
|
||||
<field name="ECEF" units="m" type="float" elements="3"/>
|
||||
<field name="RNE" units="" type="float" elements="9"/>
|
||||
<field name="Be" units="" type="float" elements="3"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="10000"/>
|
||||
<logging updatemode="never" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user