rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Flight/AHRS Comms: Whitespace fixes

Browse Source 
gnuindent -npro -kr -i8 -ts8 -sob -ss -ncs -cp1 -il0 -hnl -l150

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1836 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-02 02:17:22 +00:00 committed by peabody124
parent ea3fb03c7f
commit 0fd9fda7a6
18 changed files with 638 additions and 891 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
flight/Libraries/CoordinateConversions.c
View File

@ -71,8 +71,8 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
double x = ECEF[0], y = ECEF[1], z = ECEF[2];
double Lat, N, NplusH, delta, esLat;
uint16_t iter;
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11 // used to be e-14, but we don't need sub micrometer exact calculations
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11 // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG * atan2(y, x);
Lat = DEG2RAD * LLA[0];
@ -84,11 +84,7 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3])
while (((delta > ACCURACY) || (delta < -ACCURACY))
&& (iter < MAX_ITER)) {
delta =
Lat -
atan(z /
(sqrt(x * x + y * y) *
(1 - (N * e * e / NplusH))));
delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
@ -176,8 +172,7 @@ void RPY2Quaternion(float rpy[3], float q[4])
void Quaternion2R(float q[4], float Rbe[3][3])
{
float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s =
q[2] * q[2], q3s = q[3] * q[3];
float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
Rbe[0][0] = q0s + q1s - q2s - q3s;
Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
@ -191,8 +186,7 @@ void Quaternion2R(float q[4], float Rbe[3][3])
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3],
float NED[3])
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];
float diff[3];
@ -203,20 +197,13 @@ void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3],
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
NED[0] =
Rne[0][0] * diff[0] + Rne[0][1] * diff[1] +
Rne[0][2] * diff[2];
NED[1] =
Rne[1][0] * diff[0] + Rne[1][1] * diff[1] +
Rne[1][2] * diff[2];
NED[2] =
Rne[2][0] * diff[0] + Rne[2][1] * diff[1] +
Rne[2][2] * diff[2];
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3],
float NED[3])
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
float diff[3];
@ -224,13 +211,7 @@ void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3],
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
NED[0] =
Rne[0][0] * diff[0] + Rne[0][1] * diff[1] +
Rne[0][2] * diff[2];
NED[1] =
Rne[1][0] * diff[0] + Rne[1][1] * diff[1] +
Rne[1][2] * diff[2];
NED[2] =
Rne[2][0] * diff[0] + Rne[2][1] * diff[1] +
Rne[2][2] * diff[2];
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
}

301
flight/Libraries/WorldMagModel.c
View File

@ -72,8 +72,7 @@ int WMM_Initialize()
// Sets Magnetic Model parameters
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;
// Really, Really needs to be read from a file - out of date in 2015 at latest
@ -84,27 +83,19 @@ int WMM_Initialize()
return 0;
}
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid,
uint16_t Month, uint16_t Day, uint16_t Year,
float B[3])
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3])
{
char Error_Message[255];
Ellip =
(WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid));
MagneticModel =
(WMMtype_MagneticModel *)
Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid));
MagneticModel = (WMMtype_MagneticModel *)
pvPortMalloc(sizeof(WMMtype_MagneticModel));
WMMtype_CoordSpherical *CoordSpherical =
(WMMtype_CoordSpherical *)
WMMtype_CoordSpherical *CoordSpherical = (WMMtype_CoordSpherical *)
pvPortMalloc(sizeof(CoordSpherical));
WMMtype_CoordGeodetic *CoordGeodetic =
(WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic));
WMMtype_Date *Date =
(WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date));
WMMtype_GeoMagneticElements *GeoMagneticElements =
(WMMtype_GeoMagneticElements *)
WMMtype_CoordGeodetic *CoordGeodetic = (WMMtype_CoordGeodetic *) pvPortMalloc(sizeof(CoordGeodetic));
WMMtype_Date *Date = (WMMtype_Date *) pvPortMalloc(sizeof(WMMtype_Date));
WMMtype_GeoMagneticElements *GeoMagneticElements = (WMMtype_GeoMagneticElements *)
pvPortMalloc(sizeof(GeoMagneticElements));
WMM_Initialize();
@ -133,9 +124,7 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid,
vPortFree(GeoMagneticElements);
}
uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
WMMtype_CoordGeodetic * CoordGeodetic,
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
@ -162,8 +151,7 @@ uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
{
WMMtype_LegendreFunction LegendreFunction;
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_AssociatedLegendreFunction(CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */
@ -177,10 +165,7 @@ uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical,
}
uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *
CoordSpherical,
uint16_t nMax,
WMMtype_SphericalHarmonicVariables
* SphVariables)
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
@ -211,15 +196,9 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *
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);
}
/*
@ -233,20 +212,13 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *
SphVariables->cos_mlambda[1] = cos_lambda;
SphVariables->sin_mlambda[1] = sin_lambda;
for (m = 2; m <= nMax; m++) {
SphVariables->cos_mlambda[m] =
SphVariables->cos_mlambda[m - 1] * cos_lambda -
SphVariables->sin_mlambda[m - 1] * sin_lambda;
SphVariables->sin_mlambda[m] =
SphVariables->cos_mlambda[m - 1] * sin_lambda +
SphVariables->sin_mlambda[m - 1] * cos_lambda;
SphVariables->cos_mlambda[m] = SphVariables->cos_mlambda[m - 1] * cos_lambda - SphVariables->sin_mlambda[m - 1] * sin_lambda;
SphVariables->sin_mlambda[m] = SphVariables->cos_mlambda[m - 1] * sin_lambda + SphVariables->sin_mlambda[m - 1] * cos_lambda;
}
return TRUE;
} /*WMM_ComputeSphericalHarmonicVariables */
uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *
CoordSpherical, uint16_t nMax,
WMMtype_LegendreFunction *
LegendreFunction)
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.
@ -270,13 +242,9 @@ uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *
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);
FLAG = WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax);
else
FLAG =
WMM_PcupHigh(LegendreFunction->Pcup,
LegendreFunction->dPcup, sin_phi, nMax);
FLAG = WMM_PcupHigh(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax);
if (FLAG == 0) /* Error while computing Legendre variables */
return FALSE;
@ -285,8 +253,7 @@ uint16_t WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical *
uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
WMMtype_SphericalHarmonicVariables * SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults)
WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults)
{
/* Computes Geomagnetic Field Elements X, Y and Z in Spherical coordinate system using
spherical harmonic summation.
@ -324,11 +291,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
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];
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))
@ -337,9 +301,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
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])
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))
@ -349,9 +311,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
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])
SphVariables->cos_mlambda[m] + MagneticModel->Main_Field_Coeff_H[index] * SphVariables->sin_mlambda[m])
* LegendreFunction->dPcup[index];
}
@ -368,17 +328,14 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
*/
{
WMM_SummationSpecial(SphVariables, CoordSpherical,
MagneticResults);
WMM_SummationSpecial(SphVariables, CoordSpherical, MagneticResults);
}
return TRUE;
} /*WMM_Summation */
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults)
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
@ -407,11 +364,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
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];
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))
@ -420,9 +374,7 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
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])
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))
@ -432,9 +384,7 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
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])
SphVariables->cos_mlambda[m] + MagneticModel->Secular_Var_Coeff_H[index] * SphVariables->sin_mlambda[m])
* LegendreFunction->dPcup[index];
}
}
@ -444,18 +394,14 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
} else
/* Special calculation for component By at Geographic poles */
{
WMM_SecVarSummationSpecial(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)
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
@ -490,20 +436,13 @@ uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
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->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi);
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi);
MagneticResultsGeo->By = MagneticResultsSph->By;
return TRUE;
} /*WMM_RotateMagneticVector */
uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *
MagneticResultsGeo,
WMMtype_GeoMagneticElements *
GeoMagneticElements)
uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGeo, WMMtype_GeoMagneticElements * GeoMagneticElements)
/* Calculate all the Geomagnetic elements from X,Y and Z components
INPUT MagneticResultsGeo Pointer to data structure with the following elements
@ -525,24 +464,15 @@ uint16_t WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults *
GeoMagneticElements->Y = MagneticResultsGeo->By;
GeoMagneticElements->Z = MagneticResultsGeo->Bz;
GeoMagneticElements->H =
sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx +
MagneticResultsGeo->By * MagneticResultsGeo->By);
GeoMagneticElements->F =
sqrt(GeoMagneticElements->H * GeoMagneticElements->H +
MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
GeoMagneticElements->Decl =
RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X));
GeoMagneticElements->Incl =
RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H));
GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By);
GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X));
GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H));
return TRUE;
} /*WMM_CalculateGeoMagneticElements */
uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *
MagneticVariation,
WMMtype_GeoMagneticElements *
MagneticElements)
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 )
@ -567,17 +497,13 @@ uint16_t WMM_CalculateSecularVariation(WMMtype_MagneticResults *
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->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F;
MagneticElements->Decldot =
180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot -
MagneticElements->Y * MagneticElements->Xdot) /
(MagneticElements->H * MagneticElements->H);
MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H);
MagneticElements->Incldot =
180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot -
MagneticElements->Z * MagneticElements->Hdot) /
(MagneticElements->F * MagneticElements->F);
MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F);
MagneticElements->GVdot = MagneticElements->Decldot;
return TRUE;
} /*WMM_CalculateSecularVariation */
@ -646,14 +572,8 @@ uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
f2[k] = (float)(n - 1) / (float)(n);
for (m = 1; m <= n - 2; m++) {
k = k + 1;
f1[k] =
(float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n -
m];
f2[k] =
PreSqr[n - m - 1] * PreSqr[n + m -
1] / PreSqr[n +
m] /
PreSqr[n - m];
f1[k] = (float)(2 * n - 1) / PreSqr[n + m] / PreSqr[n - m];
f2[k] = PreSqr[n - m - 1] * PreSqr[n + m - 1] / PreSqr[n + m] / PreSqr[n - m];
}
k = k + 2;
}
@ -696,18 +616,13 @@ uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
k = kstart + m + 1;
pm1 = x * PreSqr[2 * m + 1] * pm2;
Pcup[k] = pm1 * rescalem;
dPcup[k] =
((pm2 * rescalem) * PreSqr[2 * m + 1] -
x * (float)(m + 1) * Pcup[k]) / z;
dPcup[k] = ((pm2 * rescalem) * PreSqr[2 * m + 1] - x * (float)(m + 1) * Pcup[k]) / z;
/* Calculate Pcup(n,m) */
for (n = m + 2; n <= nMax; ++n) {
k = k + n;
plm = x * f1[k] * pm1 - f2[k] * pm2;
Pcup[k] = plm * rescalem;
dPcup[k] =
(PreSqr[n + m] * PreSqr[n - m] *
(pm1 * rescalem) -
(float)(n) * x * Pcup[k]) / z;
dPcup[k] = (PreSqr[n + m] * PreSqr[n - m] * (pm1 * rescalem) - (float)(n) * x * Pcup[k]) / z;
pm2 = pm1;
pm1 = plm;
}
@ -763,37 +678,22 @@ uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
if (n == m) {
index1 = (n - 1) * n / 2 + m - 1;
Pcup[index] = z * Pcup[index1];
dPcup[index] =
z * dPcup[index1] + x * Pcup[index1];
dPcup[index] = z * dPcup[index1] + x * Pcup[index1];
} else if (n == 1 && m == 0) {
index1 = (n - 1) * n / 2 + m;
Pcup[index] = x * Pcup[index1];
dPcup[index] =
x * dPcup[index1] - z * Pcup[index1];
dPcup[index] = x * dPcup[index1] - z * Pcup[index1];
} else if (n > 1 && n != m) {
index1 = (n - 2) * (n - 1) / 2 + m;
index2 = (n - 1) * n / 2 + m;
if (m > n - 2) {
Pcup[index] = x * Pcup[index2];
dPcup[index] =
x * dPcup[index2] -
z * Pcup[index2];
dPcup[index] = x * dPcup[index2] - z * Pcup[index2];
} else {
k = (float)(((n - 1) * (n - 1)) -
(m * m)) / (float)((2 *
n -
1)
*
(2 *
n -
3));
Pcup[index] =
x * Pcup[index2] -
k * Pcup[index1];
dPcup[index] =
x * dPcup[index2] -
z * Pcup[index2] -
k * dPcup[index1];
k = (float)(((n - 1) * (n - 1)) - (m * m)) / (float)((2 * n - 1)
* (2 * n - 3));
Pcup[index] = x * Pcup[index2] - k * Pcup[index1];
dPcup[index] = x * dPcup[index2] - z * Pcup[index2] - k * dPcup[index1];
}
}
}
@ -807,17 +707,12 @@ uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
index = (n * (n + 1) / 2);
index1 = (n - 1) * n / 2;
/* for m = 0 */
schmidtQuasiNorm[index] =
schmidtQuasiNorm[index1] * (float)(2 * n -
1) / (float)n;
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * (float)(2 * n - 1) / (float)n;
for (m = 1; m <= n; m++) {
index = (n * (n + 1) / 2 + m);
index1 = (n * (n + 1) / 2 + m - 1);
schmidtQuasiNorm[index] =
schmidtQuasiNorm[index1] *
sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) /
(float)(n + m));
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m));
}
}
@ -829,10 +724,8 @@ uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
for (n = 1; n <= nMax; n++) {
for (m = 0; m <= n; m++) {
index = (n * (n + 1) / 2 + m);
Pcup[index] =
Pcup[index] * schmidtQuasiNorm[index];
dPcup[index] =
-dPcup[index] * schmidtQuasiNorm[index];
Pcup[index] = Pcup[index] * schmidtQuasiNorm[index];
dPcup[index] = -dPcup[index] * schmidtQuasiNorm[index];
/* The sign is changed since the new WMM routines use derivative with respect to latitude
insted of co-latitude */
}
@ -842,9 +735,7 @@ uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
} /*WMM_PcupLow */
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults)
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
@ -857,8 +748,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
*/
{
uint16_t n, index;
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1,
schmidtQuasiNorm2, schmidtQuasiNorm3;
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3;
PcupS[0] = 1;
schmidtQuasiNorm1 = 1.0;
@ -873,20 +763,14 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */
index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 =
schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 =
schmidtQuasiNorm2 * sqrt((float)(n * 2) /
(float)(n + 1));
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2;
if (n == 1) {
PcupS[n] = PcupS[n - 1];
} else {
k = (float)(((n - 1) * (n - 1)) -
1) / (float)((2 * n - 1) * (2 * n -
3));
PcupS[n] =
sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3));
PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
}
/* 1 nMax (n+2) n m m m
@ -897,9 +781,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
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])
SphVariables->sin_mlambda[1] - MagneticModel->Main_Field_Coeff_H[index] * SphVariables->cos_mlambda[1])
* PcupS[n] * schmidtQuasiNorm3;
}
@ -907,11 +789,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
} /*WMM_SummationSpecial */
uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical *
CoordSpherical,
WMMtype_MagneticResults *
MagneticResults)
SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults)
{
/*Special calculation for the secular variation summation at the poles.
@ -923,8 +801,7 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
*/
uint16_t n, index;
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1,
schmidtQuasiNorm2, schmidtQuasiNorm3;
float k, sin_phi, PcupS[NUMPCUPS], schmidtQuasiNorm1, schmidtQuasiNorm2, schmidtQuasiNorm3;
PcupS[0] = 1;
schmidtQuasiNorm1 = 1.0;
@ -934,20 +811,14 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) {
index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 =
schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 =
schmidtQuasiNorm2 * sqrt((float)(n * 2) /
(float)(n + 1));
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2;
if (n == 1) {
PcupS[n] = PcupS[n - 1];
} else {
k = (float)(((n - 1) * (n - 1)) -
1) / (float)((2 * n - 1) * (2 * n -
3));
PcupS[n] =
sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
k = (float)(((n - 1) * (n - 1)) - 1) / (float)((2 * n - 1) * (2 * n - 3));
PcupS[n] = sin_phi * PcupS[n - 1] - k * PcupS[n - 2];
}
/* 1 nMax (n+2) n m m m
@ -958,9 +829,7 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
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])
SphVariables->sin_mlambda[1] - MagneticModel->Secular_Var_Coeff_H[index] * SphVariables->cos_mlambda[1])
* PcupS[n] * schmidtQuasiNorm3;
}
@ -981,15 +850,9 @@ void WMM_TimelyModifyMagneticModel(WMMtype_Date * UserDate)
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];
(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];
(UserDate->DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index];
}
}
}
@ -999,8 +862,7 @@ uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error)
// Converts a given calendar date into a decimal year
{
uint16_t temp = 0; // Total number of days
uint16_t MonthDays[13] =
{ 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
uint16_t MonthDays[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
uint16_t ExtraDay = 0;
uint16_t i;
@ -1011,12 +873,10 @@ uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error)
/******************Validation********************************/
if (CalendarDate->Month <= 0 || CalendarDate->Month > 12) {
strcpy(Error,
"\nError: The Month entered is invalid, valid months are '1 to 12'\n");
strcpy(Error, "\nError: The Month entered is invalid, valid months are '1 to 12'\n");
return 0;
}
if (CalendarDate->Day <= 0
|| CalendarDate->Day > MonthDays[CalendarDate->Month]) {
if (CalendarDate->Day <= 0 || CalendarDate->Day > MonthDays[CalendarDate->Month]) {
// printf("\nThe number of days in month %d is %d\n", CalendarDate->Month, MonthDays[CalendarDate->Month]);
strcpy(Error, "\nError: The day entered is invalid\n");
return 0;
@ -1025,14 +885,12 @@ uint16_t WMM_DateToYear(WMMtype_Date * CalendarDate, char *Error)
for (i = 1; i <= CalendarDate->Month; i++)
temp += MonthDays[i - 1];
temp += CalendarDate->Day;
CalendarDate->DecimalYear =
CalendarDate->Year + (temp - 1) / (365.0 + ExtraDay);
CalendarDate->DecimalYear = CalendarDate->Year + (temp - 1) / (365.0 + ExtraDay);
return 1;
} /*WMM_DateToYear */
void WMM_GeodeticToSpherical(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
@ -1049,8 +907,7 @@ void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic,
// compute ECEF Cartesian coordinates of specified point (for longitude=0)
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

98
flight/Libraries/ahrs_comm_objects.c
View File

@ -24,18 +24,18 @@ AhrsSharedObject objectHandles[MAX_AHRS_OBJECTS];
#define CREATEHANDLE(n,obj) \
void * obj##Handle() {return (&objectHandles[n]);}
CREATEHANDLE( 0, AttitudeRaw );
CREATEHANDLE( 1, AttitudeActual );
CREATEHANDLE( 2, AHRSCalibration );
CREATEHANDLE( 3, AttitudeSettings );
CREATEHANDLE( 4, AhrsStatus );
CREATEHANDLE( 5, BaroAltitude );
CREATEHANDLE( 6, GPSPosition );
CREATEHANDLE( 7, PositionActual );
CREATEHANDLE( 8, HomeLocation );
CREATEHANDLE( 9, AHRSSettings );
CREATEHANDLE(0, AttitudeRaw);
CREATEHANDLE(1, AttitudeActual);
CREATEHANDLE(2, AHRSCalibration);
CREATEHANDLE(3, AttitudeSettings);
CREATEHANDLE(4, AhrsStatus);
CREATEHANDLE(5, BaroAltitude);
CREATEHANDLE(6, GPSPosition);
CREATEHANDLE(7, PositionActual);
CREATEHANDLE(8, HomeLocation);
CREATEHANDLE(9, AHRSSettings);
#if 10 != MAX_AHRS_OBJECTS //sanity check
#if 10 != MAX_AHRS_OBJECTS //sanity check
#error We did not create the correct number of xxxHandle() functions
#endif
@ -60,79 +60,73 @@ static void ObjectUpdatedCb(UAVObjEvent * ev);
#endif
void AhrsInitHandles( void )
void AhrsInitHandles(void)
{
int idx = 0;
//Note: the first item in this list has the highest priority
//the last has the lowest priority
ADDHANDLE( idx++, AttitudeRaw );
ADDHANDLE( idx++, AttitudeActual );
ADDHANDLE( idx++, AHRSCalibration );
ADDHANDLE( idx++, AttitudeSettings );
ADDHANDLE( idx++, AhrsStatus );
ADDHANDLE( idx++, BaroAltitude );
ADDHANDLE( idx++, GPSPosition );
ADDHANDLE( idx++, PositionActual );
ADDHANDLE( idx++, HomeLocation );
ADDHANDLE( idx++, AHRSSettings );
if( idx != MAX_AHRS_OBJECTS ) {
PIOS_DEBUG_Assert( 0 );
ADDHANDLE(idx++, AttitudeRaw);
ADDHANDLE(idx++, AttitudeActual);
ADDHANDLE(idx++, AHRSCalibration);
ADDHANDLE(idx++, AttitudeSettings);
ADDHANDLE(idx++, AhrsStatus);
ADDHANDLE(idx++, BaroAltitude);
ADDHANDLE(idx++, GPSPosition);
ADDHANDLE(idx++, PositionActual);
ADDHANDLE(idx++, HomeLocation);
ADDHANDLE(idx++, AHRSSettings);
if (idx != MAX_AHRS_OBJECTS) {
PIOS_DEBUG_Assert(0);
}
//Note: Only connect objects that the AHRS needs to read
//When the AHRS writes to these the data does a round trip
//AHRS->OP->AHRS due to these events
#ifndef IN_AHRS
AHRSSettingsConnectCallback(ObjectUpdatedCb);
BaroAltitudeConnectCallback(ObjectUpdatedCb);
GPSPositionConnectCallback(ObjectUpdatedCb);
HomeLocationConnectCallback(ObjectUpdatedCb);
AHRSCalibrationConnectCallback(ObjectUpdatedCb);
AHRSSettingsConnectCallback(ObjectUpdatedCb);
BaroAltitudeConnectCallback(ObjectUpdatedCb);
GPSPositionConnectCallback(ObjectUpdatedCb);
HomeLocationConnectCallback(ObjectUpdatedCb);
AHRSCalibrationConnectCallback(ObjectUpdatedCb);
#endif
}
AhrsObjHandle AhrsFromIndex( uint8_t index )
AhrsObjHandle AhrsFromIndex(uint8_t index)
{
if( index >= MAX_AHRS_OBJECTS ) {
return( NULL );
if (index >= MAX_AHRS_OBJECTS) {
return (NULL);
}
return( &objectHandles[index] );
return (&objectHandles[index]);
}
#ifndef IN_AHRS
AhrsObjHandle AhrsFromUAV( UAVObjHandle obj )
AhrsObjHandle AhrsFromUAV(UAVObjHandle obj)
{
if( objectHandles[0].uavHandle == NULL ) { //Oops - we haven't been initialised!
PIOS_DEBUG_Assert( 0 );
if (objectHandles[0].uavHandle == NULL) { //Oops - we haven't been initialised!
PIOS_DEBUG_Assert(0);
}
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
if( objectHandles[ct].uavHandle == obj ) {
return( &objectHandles[ct] );
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
if (objectHandles[ct].uavHandle == obj) {
return (&objectHandles[ct]);
}
}
return( NULL );
return (NULL);
}
/** Callback to update AHRS from UAVObjects
*/
static void ObjectUpdatedCb(UAVObjEvent * ev)
{
if(!(ev->event & EV_MASK_ALL_UPDATES))
{
return;
}
if (!(ev->event & EV_MASK_ALL_UPDATES)) {
return;
}
AhrsObjHandle hdl = AhrsFromUAV(ev->obj);
if(hdl)
{
AhrsSharedData data; //this is guaranteed to be big enough
UAVObjGetData(ev->obj,&data);
if (hdl) {
AhrsSharedData data; //this is guaranteed to be big enough
UAVObjGetData(ev->obj, &data);
AhrsSetData(hdl, &data);
}
}
#endif

227
flight/Libraries/ahrs_spi_comm.c
View File

@ -10,7 +10,6 @@
#include "STM32103CB_AHRS.h"
#endif
/*transmit and receive packet magic numbers.
These numbers are chosen to be very unlikely to occur due to noise.
CRC8 does not always catch noise from cross-coupling between data lines.
@ -23,15 +22,13 @@ CRC8 does not always catch noise from cross-coupling between data lines.
#define TXMAGIC 0x3C5AA50F
#endif
//packet types
typedef enum {COMMS_NULL, COMMS_OBJECT, COMMS_PROGRAM} COMMSCOMMAND;
typedef enum { COMMS_NULL, COMMS_OBJECT, COMMS_PROGRAM } COMMSCOMMAND;
//The maximum number of objects that can be updated in one cycle.
//Currently the link is capable of sending 3 packets per cycle but 2 is enough
#define MAX_UPDATE_OBJECTS 3
//Number of transmissions + 1 before we expect to see the data acknowledge
//This is controlled by the SPI hardware.
#define ACK_LATENCY 4
@ -52,24 +49,22 @@ typedef struct {
uint32_t magicNumber;
COMMSCOMMAND command;
AhrsEndStatus status;
union { //allow for expansion to other packet types.
union { //allow for expansion to other packet types.
ObjectPacketData objects[MAX_UPDATE_OBJECTS];
};
} CommsDataPacket;
static void FillObjectPacket();
static void CommsCallback( uint8_t crc_ok, uint8_t crc_val );
static void SetObjectDirty( const int idx );
static void CommsCallback(uint8_t crc_ok, uint8_t crc_val);
static void SetObjectDirty(const int idx);
static void HandleObjectPacket();
static void HandleRxPacket();
static void PollEvents();
#ifndef IN_AHRS
static void SendPacket( void );
static void AhrsUpdatedCb( AhrsObjHandle handle );
static void SendPacket(void);
static void AhrsUpdatedCb(AhrsObjHandle handle);
#endif
/** Transmit data buffer
*/
static CommsDataPacket txPacket;
@ -86,7 +81,6 @@ static unsigned int dirtyObjects[MAX_AHRS_OBJECTS];
*/
static bool readyObjects[MAX_AHRS_OBJECTS];
/** List of event callbacks
*/
static AhrsEventCallback objCallbacks[MAX_AHRS_OBJECTS];
@ -106,63 +100,60 @@ static uint8_t linkOk = false;
static int okCount = MIN_OK_FRAMES;
static int emptyCount = MIN_EMPTY_OBJECTS;
void AhrsInitComms( void )
void AhrsInitComms(void)
{
AhrsInitHandles();
memset( objCallbacks, 0, sizeof( AhrsEventCallback ) * MAX_AHRS_OBJECTS );
memset( callbackPending, 0, sizeof( bool ) * MAX_AHRS_OBJECTS );
memset( dirtyObjects, 0, sizeof( unsigned int ) * MAX_AHRS_OBJECTS );
memset( &txPacket, 0, sizeof( txPacket ) );
memset( &rxPacket, 0, sizeof( rxPacket ) );
memset(&readyObjects,0,sizeof( bool ) * MAX_AHRS_OBJECTS );
memset(objCallbacks, 0, sizeof(AhrsEventCallback) * MAX_AHRS_OBJECTS);
memset(callbackPending, 0, sizeof(bool) * MAX_AHRS_OBJECTS);
memset(dirtyObjects, 0, sizeof(unsigned int) * MAX_AHRS_OBJECTS);
memset(&txPacket, 0, sizeof(txPacket));
memset(&rxPacket, 0, sizeof(rxPacket));
memset(&readyObjects, 0, sizeof(bool) * MAX_AHRS_OBJECTS);
txPacket.command = COMMS_NULL;
rxPacket.command = COMMS_NULL;
#ifdef IN_AHRS
PIOS_SPI_Init();
#else
PIOS_SPI_SetClockSpeed( PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8 ); //72MHz/16 = 4.5MHz
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
AhrsObjHandle hdl = AhrsFromIndex( ct );
if( hdl) {
AhrsConnectCallBack( hdl, AhrsUpdatedCb );
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8); //72MHz/16 = 4.5MHz
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
AhrsObjHandle hdl = AhrsFromIndex(ct);
if (hdl) {
AhrsConnectCallBack(hdl, AhrsUpdatedCb);
}
}
#endif
}
int32_t AhrsSetData( AhrsObjHandle obj, const void *dataIn )
int32_t AhrsSetData(AhrsObjHandle obj, const void *dataIn)
{
if( obj == NULL || dataIn == NULL || obj->data == NULL ) {
return( -1 );
if (obj == NULL || dataIn == NULL || obj->data == NULL) {
return (-1);
}
if( memcmp( obj->data, dataIn, obj->size ) == 0 ) { //nothing to do, don't generate an event
return( 0 );
if (memcmp(obj->data, dataIn, obj->size) == 0) { //nothing to do, don't generate an event
return (0);
}
memcpy( obj->data, dataIn, obj->size );
SetObjectDirty( obj->index );
return( 0 );
memcpy(obj->data, dataIn, obj->size);
SetObjectDirty(obj->index);
return (0);
}
int32_t AhrsGetData( AhrsObjHandle obj, void *dataOut )
int32_t AhrsGetData(AhrsObjHandle obj, void *dataOut)
{
if( obj == NULL || dataOut == NULL || obj->data == NULL ) {
return( -1 );
if (obj == NULL || dataOut == NULL || obj->data == NULL) {
return (-1);
}
memcpy( dataOut, obj->data, obj->size );
return( 0 );
memcpy(dataOut, obj->data, obj->size);
return (0);
}
/** Mark an object to be sent
*/
void SetObjectDirty( const int idx )
void SetObjectDirty(const int idx)
{
if( idx < 0 || idx >= MAX_AHRS_OBJECTS ) {
if (idx < 0 || idx >= MAX_AHRS_OBJECTS) {
return;
}
dirtyObjects[idx] = ACK_LATENCY;
@ -176,23 +167,21 @@ void FillObjectPacket()
txPacket.command = COMMS_OBJECT;
txPacket.magicNumber = TXMAGIC;
int idx = 0;
for( int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++ ) {
for (int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++) {
txPacket.objects[ct].index = AHRS_NO_OBJECT;
for( ; idx < MAX_AHRS_OBJECTS; idx++ ) {
if( dirtyObjects[idx] > 0 ) {
if( dirtyObjects[idx] == ACK_LATENCY ) {
for (; idx < MAX_AHRS_OBJECTS; idx++) {
if (dirtyObjects[idx] > 0) {
if (dirtyObjects[idx] == ACK_LATENCY) {
dirtyObjects[idx]--;
txPacket.objects[ct].index = idx;
AhrsObjHandle hdl = AhrsFromIndex( idx );
if( hdl ) {
memcpy( &txPacket.objects[ct].object,
hdl->data,
hdl->size );
AhrsObjHandle hdl = AhrsFromIndex(idx);
if (hdl) {
memcpy(&txPacket.objects[ct].object, hdl->data, hdl->size);
}
break;
} else {
dirtyObjects[idx] --;
if( dirtyObjects[idx] == 0 ) { //timed out
dirtyObjects[idx]--;
if (dirtyObjects[idx] == 0) { //timed out
dirtyObjects[idx] = ACK_LATENCY;
txPacket.status.retries++;
}
@ -206,7 +195,7 @@ void FillObjectPacket()
*/
void HandleRxPacket()
{
switch( rxPacket.command ) {
switch (rxPacket.command) {
case COMMS_NULL:
break;
@ -214,7 +203,7 @@ void HandleRxPacket()
HandleObjectPacket();
break;
case COMMS_PROGRAM: //TODO: programming protocol
case COMMS_PROGRAM: //TODO: programming protocol
break;
default:
@ -227,28 +216,28 @@ void HandleRxPacket()
*/
void HandleObjectPacket()
{
for( int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++ ) {
for (int ct = 0; ct < MAX_UPDATE_OBJECTS; ct++) {
//Flag objects that have been successfully received at the other end
uint8_t idx = rxPacket.objects[ct].done;
if( idx < MAX_AHRS_OBJECTS ) {
if (idx < MAX_AHRS_OBJECTS) {
if( dirtyObjects[idx] == 1 ) { //this ack is the correct one for the last send
if (dirtyObjects[idx] == 1) { //this ack is the correct one for the last send
dirtyObjects[idx] = 0;
}
}
txPacket.objects[ct].done = AHRS_NO_OBJECT;
idx = rxPacket.objects[ct].index;
if( idx == AHRS_NO_OBJECT ) {
if( emptyCount > 0 ) {
if (idx == AHRS_NO_OBJECT) {
if (emptyCount > 0) {
emptyCount--;
}
continue;
}
AhrsObjHandle obj = AhrsFromIndex( idx );
if( obj ) {
memcpy( obj->data, &rxPacket.objects[ct].object, obj->size );
AhrsObjHandle obj = AhrsFromIndex(idx);
if (obj) {
memcpy(obj->data, &rxPacket.objects[ct].object, obj->size);
txPacket.objects[ct].done = idx;
callbackPending[idx] = true;
readyObjects[idx] = true;
@ -257,18 +246,17 @@ void HandleObjectPacket()
}
}
#ifdef IN_AHRS
FillObjectPacket(); //ready for the next frame
FillObjectPacket(); //ready for the next frame
#endif
}
int32_t AhrsConnectCallBack( AhrsObjHandle obj, AhrsEventCallback cb )
int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb)
{
if( obj == NULL || obj->data == NULL ) {
return( -1 );
if (obj == NULL || obj->data == NULL) {
return (-1);
}
objCallbacks[obj->index] = cb;
return( 0 );
return (0);
}
AhrsCommStatus AhrsGetStatus()
@ -277,25 +265,23 @@ AhrsCommStatus AhrsGetStatus()
status.remote = rxPacket.status;
status.local = txPacket.status;
status.linkOk = linkOk;
return( status );
return (status);
}
void CommsCallback( uint8_t crc_ok, uint8_t crc_val )
void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
{
#ifndef IN_AHRS
PIOS_SPI_RC_PinSet( PIOS_OPAHRS_SPI, 1 ); //signal the end of the transfer
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1); //signal the end of the transfer
#endif
txPacket.command = COMMS_NULL; //we must send something so default to null
if(rxPacket.magicNumber != RXMAGIC)
{
txPacket.command = COMMS_NULL; //we must send something so default to null
if (rxPacket.magicNumber != RXMAGIC) {
crc_ok = false;
}
rxPacket.magicNumber = 0;
if( crc_ok ) {
if(!linkOk && okCount > 0 ) {
if (crc_ok) {
if (!linkOk && okCount > 0) {
okCount--;
if( okCount == 0 ) {
if (okCount == 0) {
linkOk = true;
okCount = MAX_CRC_ERRORS;
emptyCount = MIN_EMPTY_OBJECTS;
@ -303,15 +289,15 @@ void CommsCallback( uint8_t crc_ok, uint8_t crc_val )
}
HandleRxPacket();
} else {
#ifdef IN_AHRS //AHRS - do we neeed to enter program mode?
if( memcmp( &rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH ) == 0 ) {
#ifdef IN_AHRS //AHRS - do we neeed to enter program mode?
if (memcmp(&rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
AhrsProgramReceive();
}
#endif
txPacket.status.crcErrors++;
if(linkOk && okCount > 0 ) {
if (linkOk && okCount > 0) {
okCount--;
if( okCount == 0 ) {
if (okCount == 0) {
linkOk = false;
okCount = MIN_OK_FRAMES;
}
@ -319,24 +305,22 @@ void CommsCallback( uint8_t crc_ok, uint8_t crc_val )
}
#ifdef IN_AHRS
/*queue next frame
If PIOS_SPI_TransferBlock() fails for any reason, comms will stop working.
In that case, AhrsPoll() should kick start things again.
*/
PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
If PIOS_SPI_TransferBlock() fails for any reason, comms will stop working.
In that case, AhrsPoll() should kick start things again.
*/
PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
#endif
}
void PollEvents( void )
void PollEvents(void)
{
for( int idx = 0; idx < MAX_AHRS_OBJECTS; idx++ ) {
if( objCallbacks[idx] ) {
for (int idx = 0; idx < MAX_AHRS_OBJECTS; idx++) {
if (objCallbacks[idx]) {
PIOS_IRQ_Disable();
if( callbackPending[idx] ) {
if (callbackPending[idx]) {
callbackPending[idx] = false;
PIOS_IRQ_Enable();
objCallbacks[idx]( AhrsFromIndex( idx ) );
objCallbacks[idx] (AhrsFromIndex(idx));
} else {
PIOS_IRQ_Enable();
}
@ -344,57 +328,50 @@ void PollEvents( void )
}
}
#ifdef IN_AHRS
void AhrsPoll()
{
PollEvents();
if( PIOS_SPI_Busy( PIOS_SPI_OP ) != 0 ) { //Everything is working correctly
if (PIOS_SPI_Busy(PIOS_SPI_OP) != 0) { //Everything is working correctly
return;
}
txPacket.status.kickStarts++;
//comms have broken down - try kick starting it.
txPacket.command = COMMS_NULL; //we must send something so default to null
PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
txPacket.command = COMMS_NULL; //we must send something so default to null
PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
}
bool AhrsLinkReady( void )
bool AhrsLinkReady(void)
{
for(int ct=0; ct< MAX_AHRS_OBJECTS; ct++)
{
if(!readyObjects[ct])
{
return(false);
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
if (!readyObjects[ct]) {
return (false);
}
}
return( linkOk);
return (linkOk);
}
#else
void AhrsSendObjects( void )
void AhrsSendObjects(void)
{
static bool oldLink = false;
PollEvents();
if( oldLink != linkOk ) {
if (oldLink != linkOk) {
oldLink = linkOk;
if( linkOk ) {
for( int ct = 0; ct < MAX_AHRS_OBJECTS; ct++ ) {
AhrsObjHandle hdl = AhrsFromIndex( ct );
if( !hdl ) { //paranoid - shouldn't ever happen
if (linkOk) {
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
AhrsObjHandle hdl = AhrsFromIndex(ct);
if (!hdl) { //paranoid - shouldn't ever happen
continue;
}
AhrsSharedData data;
UAVObjGetData( hdl->uavHandle, &data );
AhrsSetData( hdl, &data );
SetObjectDirty( ct ); //force even unchanged data to be sent
UAVObjGetData(hdl->uavHandle, &data);
AhrsSetData(hdl, &data);
SetObjectDirty(ct); //force even unchanged data to be sent
}
}
}
@ -402,20 +379,16 @@ void AhrsSendObjects( void )
SendPacket();
}
void SendPacket( void )
void SendPacket(void)
{
PIOS_SPI_RC_PinSet( PIOS_OPAHRS_SPI, 0 );
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
//no point checking if this failed. There isn't much we could do about it if it did fail
PIOS_SPI_TransferBlock( PIOS_OPAHRS_SPI, ( uint8_t * )&txPacket,
( uint8_t * )&rxPacket, sizeof( CommsDataPacket ), &CommsCallback );
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
}
static void AhrsUpdatedCb( AhrsObjHandle handle )
static void AhrsUpdatedCb(AhrsObjHandle handle)
{
UAVObjSetData( handle->uavHandle, handle->data );
UAVObjSetData(handle->uavHandle, handle->data);
return;
}
#endif

49
flight/Libraries/ahrs_spi_program_master.c
View File

@ -1,26 +1,23 @@
#include "ahrs_program_master.h"
#include "ahrs_program.h"
#include "pios_spi.h"
char connectTxBuf[SPI_PROGRAM_REQUEST_LENGTH] = {SPI_PROGRAM_REQUEST};
char connectRxBuf[SPI_PROGRAM_REQUEST_LENGTH];
#define MAX_CONNECT_TRIES 10
uint32_t AhrsProgramConnect( void )
{
memset( connectRxBuf, 0, SPI_PROGRAM_REQUEST_LENGTH );
for( int ct = 0; ct < MAX_CONNECT_TRIES; ct++ ) {
uint32_t res = PIOS_SPI_TransferBlock( PIOS_SPI_OP, ( uint8_t * )&connectTxBuf,
( uint8_t * )&connectRxBuf, SPI_PROGRAM_REQUEST_LENGTH, NULL );
if( res == 0 && memcmp( connectRxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH ) == 0 ) {
return( 0 );
}
vTaskDelay( 1 / portTICK_RATE_MS );
}
return( -1 );
}
#include "ahrs_program_master.h"
#include "ahrs_program.h"
#include "pios_spi.h"
char connectTxBuf[SPI_PROGRAM_REQUEST_LENGTH] = { SPI_PROGRAM_REQUEST };
char connectRxBuf[SPI_PROGRAM_REQUEST_LENGTH];
#define MAX_CONNECT_TRIES 10
uint32_t AhrsProgramConnect(void)
{
memset(connectRxBuf, 0, SPI_PROGRAM_REQUEST_LENGTH);
for (int ct = 0; ct < MAX_CONNECT_TRIES; ct++) {
uint32_t res = PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & connectTxBuf,
(uint8_t *) & connectRxBuf, SPI_PROGRAM_REQUEST_LENGTH, NULL);
if (res == 0 && memcmp(connectRxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
return (0);
}
vTaskDelay(1 / portTICK_RATE_MS);
}
return (-1);
}

18
flight/Libraries/ahrs_spi_program_slave.c
View File

@ -1,9 +1,9 @@
#include <stdint.h>
#include "ahrs_spi_program_slave.h"
#include "ahrs_spi_program.h"
//TODO:Implement programming protocol
void AhrsProgramReceive( void )
{
}
#include <stdint.h>
#include "ahrs_spi_program_slave.h"
#include "ahrs_spi_program.h"
//TODO:Implement programming protocol
void AhrsProgramReceive(void)
{
}

12
flight/Libraries/buffer.c
View File

@ -118,8 +118,7 @@ uint8_t bufferGetFromFront(cBuffer * buffer)
* @arg -1 for success
* @arg 0 error
*/
uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest,
uint32_t size)
uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, uint32_t size)
{
if (size > buffer->datalength)
return -1;
@ -163,8 +162,7 @@ void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes)
uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index)
{
// return character at index in buffer
return buffer->dataptr[(buffer->dataindex + index) %
(buffer->size)];
return buffer->dataptr[(buffer->dataindex + index) % (buffer->size)];
}
/**
@ -180,8 +178,7 @@ uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data)
// make sure the buffer has room
if (buffer->datalength < buffer->size) {
// save data byte at end of buffer
buffer->dataptr[(buffer->dataindex + buffer->datalength) %
buffer->size] = data;
buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data;
// increment the length
buffer->datalength++;
// return success
@ -199,8 +196,7 @@ uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data)
* @arg -1 for success
* @arg 0 error
*/
uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data,
uint32_t size)
uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data, uint32_t size)
{
// TODO: replace with memcpy and logic, for now keeping it simple
for (uint32_t i = 0; i < size; i++) {

6
flight/Libraries/inc/CoordinateConversions.h
View File

@ -48,11 +48,9 @@ void RPY2Quaternion(float rpy[3], float q[4]);
void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3],
float NED[3]);
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3],
float NED[3]);
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
#endif // COORDINATECONVERSIONS_H_

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

@ -119,34 +119,20 @@ typedef struct {
// Internal Function Prototypes
void WMM_Set_Coeff_Array();
void WMM_GeodeticToSpherical(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,
WMMtype_CoordGeodetic * CoordGeodetic,
WMMtype_GeoMagneticElements * GeoMagneticElements);
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_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_CoordSpherical *
CoordSpherical,
uint16_t nMax,
WMMtype_SphericalHarmonicVariables
* SphVariables);
CoordSpherical, uint16_t nMax, WMMtype_SphericalHarmonicVariables * SphVariables);
uint16_t WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax);
@ -154,32 +140,20 @@ uint16_t WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax);
uint16_t WMM_RotateMagneticVector(WMMtype_CoordSpherical *,
WMMtype_CoordGeodetic * CoordGeodetic,
WMMtype_MagneticResults *
MagneticResultsSph,
WMMtype_MagneticResults *
MagneticResultsGeo);
WMMtype_MagneticResults * MagneticResultsSph, WMMtype_MagneticResults * MagneticResultsGeo);
uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults);
uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical *
CoordSpherical,
WMMtype_MagneticResults *
MagneticResults);
SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults);
uint16_t WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
WMMtype_SphericalHarmonicVariables * SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults);
uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
SphVariables,
WMMtype_CoordSpherical * CoordSpherical,
WMMtype_MagneticResults * MagneticResults);
SphVariables, WMMtype_CoordSpherical * CoordSpherical, WMMtype_MagneticResults * MagneticResults);
#endif /* WMMINTERNAL_H_ */

4
flight/Libraries/inc/WorldMagModel.h
View File

@ -29,8 +29,6 @@
// Exposed Function Prototypes
int WMM_Initialize();
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid,
uint16_t Month, uint16_t Day, uint16_t Year,
float B[3]);
void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]);
#endif /* WORLDMAGMODEL_H_ */

14
flight/Libraries/inc/ahrs_comm_objects.h
View File

@ -26,17 +26,16 @@ typedef union {
PositionActualData PositionActual;
HomeLocationData HomeLocation;
AHRSSettingsData AHRSSettings;
} __attribute__(( packed ) ) AhrsSharedData;
} __attribute__ ((packed)) AhrsSharedData;
/** The number of UAVObjects we will be dealing with.
*/
#define MAX_AHRS_OBJECTS 10
/** Our own version of a UAVObject.
*/
typedef struct {
void * data;
void *data;
int size;
uint8_t index;
#ifndef IN_AHRS
@ -44,24 +43,23 @@ typedef struct {
#endif
} AhrsSharedObject;
typedef AhrsSharedObject * AhrsObjHandle;
typedef AhrsSharedObject *AhrsObjHandle;
/** Initialise the object mapping.
It is important that this is called before any of the following functions.
*/
void AhrsInitHandles( void );
void AhrsInitHandles(void);
/** the AHRS object related to the given index.
Returns the AHRS object or NULL if not found
*/
AhrsObjHandle AhrsFromIndex( uint8_t index );
AhrsObjHandle AhrsFromIndex(uint8_t index);
#ifndef IN_AHRS
/** Get the AHRS object associated with the UAVObject.
Returns the AHRS object or NULL if not found
*/
AhrsObjHandle AhrsFromUAV( UAVObjHandle obj );
AhrsObjHandle AhrsFromUAV(UAVObjHandle obj);
#endif
#endif //#ifndef AHRS_COMMS_OBJECTS_H

217
flight/Libraries/inc/ahrs_spi_comm.h
View File

@ -1,111 +1,106 @@
#ifndef AHRSCOMMS_H_INCLUDED
#define AHRSCOMMS_H_INCLUDED
#ifdef IN_AHRS //AHRS only
#include <stdint.h>
#include <stdbool.h>
/** Redirect UAVObjGetData call
*/
#define UAVObjGetData(obj, data) AhrsGetData(obj, data)
/** Redirect UAVObjSetData call
*/
#define UAVObjSetData(obj, data) AhrsSetData(obj, data)
/** Redirect UAVObjConnectCallback call
Note: in AHRS, mask is unused because there is only one event type
*/
#define UAVObjConnectCallback(obj, callback, mask) AhrsConnectCallBack(obj,callback)
/** define our own UAVObjHandle
*/
typedef void *UAVObjHandle;
#else
#include "openpilot.h"
#include "uavobjectmanager.h"
#endif
#define AHRS_NO_OBJECT 0xff
#include "ahrs_comm_objects.h"
/** Status of each end of the link
*/
typedef struct { //try to keep this short and in multiples of 4 bytes
uint8_t kickStarts; //AHRS end only
uint8_t crcErrors;
uint8_t retries;
uint8_t invalidPacket;
} AhrsEndStatus;
/** AHRS comms status
*/
typedef struct {
uint8_t linkOk;
AhrsEndStatus remote;
AhrsEndStatus local;
} AhrsCommStatus;
/** Event callback, this function is called when an object changes.
*/
typedef void ( *AhrsEventCallback )( AhrsObjHandle obj );
/** Initialise comms.
Note: this must be called before you do anything else.
Comms will not start until the first call to AhrsPoll() or
AhrsSendObjects()
*/
void AhrsInitComms( void );
/** AHRS version of UAVObject xxxSetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsSetData( AhrsObjHandle obj, const void *dataIn );
/** AHRS version of UAVObject xxxGetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsGetData( AhrsObjHandle obj, void *dataOut );
/** Connect a callback for any changes to AHRS data.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsConnectCallBack( AhrsObjHandle obj, AhrsEventCallback cb );
/** Get the current link status.
Returns: the status.
Note: the remote status will only be valid if the link is up and running
*/
AhrsCommStatus AhrsGetStatus();
#ifdef IN_AHRS //slave only
/** Send the latest objects to the OP
This also polls any pending events and kick starts the DMA
if needed
*/
void AhrsPoll();
/** Check if the link is up and we have received the first batch of updates
Returns: True if the link is up and all objects are up to date
*/
bool AhrsLinkReady();
#else //master only
/** Send the latest objects to the AHRS
This also polls any pending events
*/
void AhrsSendObjects( void );
#endif
#endif //#ifndef AHRSCOMMS_H_INCLUDED
#ifndef AHRSCOMMS_H_INCLUDED
#define AHRSCOMMS_H_INCLUDED
#ifdef IN_AHRS //AHRS only
#include <stdint.h>
#include <stdbool.h>
/** Redirect UAVObjGetData call
*/
#define UAVObjGetData(obj, data) AhrsGetData(obj, data)
/** Redirect UAVObjSetData call
*/
#define UAVObjSetData(obj, data) AhrsSetData(obj, data)
/** Redirect UAVObjConnectCallback call
Note: in AHRS, mask is unused because there is only one event type
*/
#define UAVObjConnectCallback(obj, callback, mask) AhrsConnectCallBack(obj,callback)
/** define our own UAVObjHandle
*/
typedef void *UAVObjHandle;
#else
#include "openpilot.h"
#include "uavobjectmanager.h"
#endif
#define AHRS_NO_OBJECT 0xff
#include "ahrs_comm_objects.h"
/** Status of each end of the link
*/
typedef struct { //try to keep this short and in multiples of 4 bytes
uint8_t kickStarts; //AHRS end only
uint8_t crcErrors;
uint8_t retries;
uint8_t invalidPacket;
} AhrsEndStatus;
/** AHRS comms status
*/
typedef struct {
uint8_t linkOk;
AhrsEndStatus remote;
AhrsEndStatus local;
} AhrsCommStatus;
/** Event callback, this function is called when an object changes.
*/
typedef void (*AhrsEventCallback) (AhrsObjHandle obj);
/** Initialise comms.
Note: this must be called before you do anything else.
Comms will not start until the first call to AhrsPoll() or
AhrsSendObjects()
*/
void AhrsInitComms(void);
/** AHRS version of UAVObject xxxSetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsSetData(AhrsObjHandle obj, const void *dataIn);
/** AHRS version of UAVObject xxxGetData.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsGetData(AhrsObjHandle obj, void *dataOut);
/** Connect a callback for any changes to AHRS data.
Returns: 0 if ok, -1 if an error
*/
int32_t AhrsConnectCallBack(AhrsObjHandle obj, AhrsEventCallback cb);
/** Get the current link status.
Returns: the status.
Note: the remote status will only be valid if the link is up and running
*/
AhrsCommStatus AhrsGetStatus();
#ifdef IN_AHRS //slave only
/** Send the latest objects to the OP
This also polls any pending events and kick starts the DMA
if needed
*/
void AhrsPoll();
/** Check if the link is up and we have received the first batch of updates
Returns: True if the link is up and all objects are up to date
*/
bool AhrsLinkReady();
#else //master only
/** Send the latest objects to the AHRS
This also polls any pending events
*/
void AhrsSendObjects(void);
#endif
#endif //#ifndef AHRSCOMMS_H_INCLUDED

103
flight/Libraries/inc/ahrs_spi_program.h
View File

@ -1,53 +1,50 @@
#ifndef AHRS_SPI_PROGRAM_H
#define AHRS_SPI_PROGRAM_H
/* Special packets to enter programming mode.
Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long.
Pad with spaces if needed.
*/
#define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING "
#define SPI_PROGRAM_ACK "AHRS PROGRAMMING STARTED"
#define SPI_PROGRAM_REQUEST_LENGTH 24
/**Proposed programming protocol:
In the master:
1) Send a AhrsProgramPacket containing the relevant data.
Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
2) Keep sending PROGRAM_NULL packets and wait for an ack.
Time out if we waited too long.
3) Compare ack packet with transmitted packet. The data
should be the bitwise inverse of the data transmitted.
4) repeat for next packet until finished
5) Repeat using verify packets with data all zeros
Returned data should be exactly as read from memory
In the slave:
1) Wait for an AhrsProgramPacket
2) Check CRC then write to memory
3) Bitwise invert data
4) Transmit packet.
5) repeat until we receive a verify packet
6) verify until we receive a reboot packet
Reboot packets had better have some sort of magic number in the data,
just to be absolutely sure
*/
typedef enum {PROGRAM_NULL, PROGRAM_DATA, PROGRAM_ACK, PROGRAM_VERIFY, PROGRAM_REBOOT} ProgramType;
#define SPI_MAX_PROGRAM_DATA_SIZE 256 //max 256 bytes per packet
/** Proposed program packet defintion
*/
typedef struct {
ProgramType type;
uint32_t address; //base address to place data
uint32_t size; //Size of data
uint8_t data [SPI_MAX_PROGRAM_DATA_SIZE];
uint32_t crc; //CRC32 - hardware CRC8 can be fooled
} AhrsProgramPacket;
#endif
#ifndef AHRS_SPI_PROGRAM_H
#define AHRS_SPI_PROGRAM_H
/* Special packets to enter programming mode.
Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long.
Pad with spaces if needed.
*/
#define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING "
#define SPI_PROGRAM_ACK "AHRS PROGRAMMING STARTED"
#define SPI_PROGRAM_REQUEST_LENGTH 24
/**Proposed programming protocol:
In the master:
1) Send a AhrsProgramPacket containing the relevant data.
Note crc is a CRC32 as the CRC8 used in hardware can be fooled.
2) Keep sending PROGRAM_NULL packets and wait for an ack.
Time out if we waited too long.
3) Compare ack packet with transmitted packet. The data
should be the bitwise inverse of the data transmitted.
4) repeat for next packet until finished
5) Repeat using verify packets with data all zeros
Returned data should be exactly as read from memory
In the slave:
1) Wait for an AhrsProgramPacket
2) Check CRC then write to memory
3) Bitwise invert data
4) Transmit packet.
5) repeat until we receive a verify packet
6) verify until we receive a reboot packet
Reboot packets had better have some sort of magic number in the data,
just to be absolutely sure
*/
typedef enum { PROGRAM_NULL, PROGRAM_DATA, PROGRAM_ACK, PROGRAM_VERIFY, PROGRAM_REBOOT } ProgramType;
#define SPI_MAX_PROGRAM_DATA_SIZE 256 //max 256 bytes per packet
/** Proposed program packet defintion
*/
typedef struct {
ProgramType type;
uint32_t address; //base address to place data
uint32_t size; //Size of data
uint8_t data[SPI_MAX_PROGRAM_DATA_SIZE];
uint32_t crc; //CRC32 - hardware CRC8 can be fooled
} AhrsProgramPacket;
#endif

22
flight/Libraries/inc/ahrs_spi_program_master.h
View File

@ -1,11 +1,11 @@
#ifndef AHRS_PROGRAM_MASTER_H
#define AHRS_PROGRAM_MASTER_H
/** Connect to AHRS and request programming mode
* returns: 0 if connected, -1 if failed.
*/
uint32_t AhrsProgramConnect(void);
//TODO: Implement programming protocol
#endif //AHRS_PROGRAM_MASTER_H
#ifndef AHRS_PROGRAM_MASTER_H
#define AHRS_PROGRAM_MASTER_H
/** Connect to AHRS and request programming mode
* returns: 0 if connected, -1 if failed.
*/
uint32_t AhrsProgramConnect(void);
//TODO: Implement programming protocol
#endif //AHRS_PROGRAM_MASTER_H

10
flight/Libraries/inc/ahrs_spi_program_slave.h
View File

@ -1,5 +1,5 @@
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
#define AHRS_SPI_PROGRAM_SLAVE_H
void AhrsProgramReceive(void);
#endif //AHRS_PROGRAM_SLAVE_H
#ifndef AHRS_SPI_PROGRAM_SLAVE_H
#define AHRS_SPI_PROGRAM_SLAVE_H
void AhrsProgramReceive(void);
#endif //AHRS_PROGRAM_SLAVE_H

6
flight/Libraries/inc/buffer.h
View File

@ -77,8 +77,7 @@ uint8_t bufferGetFromFront(cBuffer * buffer);
uint32_t bufferBufferedData(cBuffer * buffer);
//! copy number of elements into another buffer
uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest,
uint32_t size);
uint8_t bufferGetChunkFromFront(cBuffer * buffer, uint8_t * dest, uint32_t size);
//! dump (discard) the first numbytes from the front of the buffer
void bufferDumpFromFront(cBuffer * buffer, uint32_t numbytes);
@ -91,8 +90,7 @@ uint8_t bufferGetAtIndex(cBuffer * buffer, uint32_t index);
uint8_t bufferAddToEnd(cBuffer * buffer, uint8_t data);
//! queue a block of character to end of buffer
uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data,
uint32_t size);
uint8_t bufferAddChunkToEnd(cBuffer * buffer, const uint8_t * data, uint32_t size);
//! check if the buffer is full/not full (returns non-zero value if not full)
uint8_t bufferIsNotFull(cBuffer * buffer);

276
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -1,142 +1,134 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @brief Handles communication with AHRS and updating position
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file ahrs_comms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: As defined in PiOS/inc/pios_ahrs_comms.h
* Output objects: As defined in PiOS/inc/pios_ahrs_comms.h
*
* This module will periodically update the values of latest attitude solution
* and other objects that are transferred to and from the AHRS
* The module settings can configure how often AHRS is polled for a new solution.
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "ahrs_comms.h"
#include "ahrs_spi_comm.h"
// Private constants
#define STACK_SIZE 1400
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
// Private types
// Private variables
static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void* parameters);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AhrsInitComms();
// Start main task
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void* parameters)
{
portTickType lastSysTime;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/*Until AHRS connects, assume it doesn't know home */
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
//data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
// Main task loop
while (1)
{
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
AhrsSendObjects();
AhrsCommStatus stat = AhrsGetStatus();
if(stat.linkOk)
{
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
}else
{
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
}
AhrsStatusData sData;
AhrsStatusGet(&sData);
sData.LinkRunning = stat.linkOk;
sData.AhrsKickstarts = stat.remote.kickStarts;
sData.AhrsCrcErrors = stat.remote.crcErrors;
sData.AhrsRetries = stat.remote.retries;
sData.AhrsInvalidPackets = stat.remote.invalidPacket;
sData.OpCrcErrors = stat.local.crcErrors;
sData.OpRetries = stat.local.retries;
sData.OpInvalidPackets = stat.local.invalidPacket;
AhrsStatusSet(&sData);
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
}
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @brief Handles communication with AHRS and updating position
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file ahrs_comms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: As defined in PiOS/inc/pios_ahrs_comms.h
* Output objects: As defined in PiOS/inc/pios_ahrs_comms.h
*
* This module will periodically update the values of latest attitude solution
* and other objects that are transferred to and from the AHRS
* The module settings can configure how often AHRS is polled for a new solution.
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "ahrs_comms.h"
#include "ahrs_spi_comm.h"
// Private constants
#define STACK_SIZE 1400
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
// Private types
// Private variables
static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void *parameters);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AhrsInitComms();
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void *parameters)
{
portTickType lastSysTime;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/*Until AHRS connects, assume it doesn't know home */
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
//data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
// Main task loop
while (1) {
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
AhrsSendObjects();
AhrsCommStatus stat = AhrsGetStatus();
if (stat.linkOk) {
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
}
AhrsStatusData sData;
AhrsStatusGet(&sData);
sData.LinkRunning = stat.linkOk;
sData.AhrsKickstarts = stat.remote.kickStarts;
sData.AhrsCrcErrors = stat.remote.crcErrors;
sData.AhrsRetries = stat.remote.retries;
sData.AhrsInvalidPackets = stat.remote.invalidPacket;
sData.OpCrcErrors = stat.local.crcErrors;
sData.OpRetries = stat.local.retries;
sData.OpInvalidPackets = stat.local.invalidPacket;
AhrsStatusSet(&sData);
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
}
}
/**
* @}
* @}
*/

75
flight/OpenPilot/Modules/AHRSComms/inc/ahrs_comms.h
View File

@ -1,38 +1,37 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @{
*
* @file ahrs_comms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_COMMS_H
#define AHRS_COMMS_H
#include "openpilot.h"
int32_t AHRSCommsInitialize(void);
#endif // AHRS_COMMS_H
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @{
*
* @file ahrs_comms.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRS_COMMS_H
#define AHRS_COMMS_H
#include "openpilot.h"
int32_t AHRSCommsInitialize(void);
#endif // AHRS_COMMS_H