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

OP-908 Cleanup for unwanted double precision math on float data.

Browse Source 
- Use float counterpart instead of double precision function when dealing with float data
- Force float data type for decimal constants

+review OPReview
This commit is contained in:
Alessio Morale 2013-04-06 16:16:23 +02:00
parent 9b11ef2111
commit ce6f84063c
10 changed files with 85 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/Libraries/CoordinateConversions.c
View File

@ -73,7 +73,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
float Lat, N, NplusH, delta, esLat; float Lat, N, NplusH, delta, esLat;
uint16_t iter; uint16_t iter;
#define MAX_ITER 10 // should not take more than 5 for valid coordinates #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 ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG * atan2f(y, x); LLA[1] = RAD2DEG * atan2f(y, x);
Lat = DEG2RAD * LLA[0]; Lat = DEG2RAD * LLA[0];
@ -294,13 +294,13 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
// The first rows of rot matrices chosen in direction of v1 // The first rows of rot matrices chosen in direction of v1
mag = VectorMagnitude(v1b); mag = VectorMagnitude(v1b);
if (fabs(mag) < 1e-30) if (fabsf(mag) < 1e-30f)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rib[0][i]=v1b[i]/mag; Rib[0][i]=v1b[i]/mag;
mag = VectorMagnitude(v1e); mag = VectorMagnitude(v1e);
if (fabs(mag) < 1e-30) if (fabsf(mag) < 1e-30f)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rie[0][i]=v1e[i]/mag; Rie[0][i]=v1e[i]/mag;
@ -308,14 +308,14 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
// The second rows of rot matrices chosen in direction of v1xv2 // The second rows of rot matrices chosen in direction of v1xv2
CrossProduct(v1b,v2b,&Rib[1][0]); CrossProduct(v1b,v2b,&Rib[1][0]);
mag = VectorMagnitude(&Rib[1][0]); mag = VectorMagnitude(&Rib[1][0]);
if (fabs(mag) < 1e-30) if (fabsf(mag) < 1e-30f)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rib[1][i]=Rib[1][i]/mag; Rib[1][i]=Rib[1][i]/mag;
CrossProduct(v1e,v2e,&Rie[1][0]); CrossProduct(v1e,v2e,&Rie[1][0]);
mag = VectorMagnitude(&Rie[1][0]); mag = VectorMagnitude(&Rie[1][0]);
if (fabs(mag) < 1e-30) if (fabsf(mag) < 1e-30f)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rie[1][i]=Rie[1][i]/mag; Rie[1][i]=Rie[1][i]/mag;

116
flight/Libraries/WorldMagModel.c
View File

@ -242,7 +242,7 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
{ {
CoordGeodetic->lambda = Lon; CoordGeodetic->lambda = Lon;
CoordGeodetic->phi = Lat; CoordGeodetic->phi = Lat;
CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0; // convert to km CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km
// Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report // Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report
if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0)
@ -293,9 +293,9 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
Ellip = NULL; Ellip = NULL;
} }
B[0] = GeoMagneticElements->X * 1e-2; B[0] = GeoMagneticElements->X * 1e-2f;
B[1] = GeoMagneticElements->Y * 1e-2; B[1] = GeoMagneticElements->Y * 1e-2f;
B[2] = GeoMagneticElements->Z * 1e-2; B[2] = GeoMagneticElements->Z * 1e-2f;
return returned; return returned;
} }
@ -433,8 +433,8 @@ int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical
float cos_lambda, sin_lambda; float cos_lambda, sin_lambda;
uint16_t m, n; uint16_t m, n;
cos_lambda = cos(DEG2RAD(CoordSpherical->lambda)); cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda));
sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda));
/* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2)
for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */
@ -444,12 +444,12 @@ int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical
SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r); SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r);
/* /*
Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax
cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b) cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b)
sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b) sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b)
*/ */
SphVariables->cos_mlambda[0] = 1.0; SphVariables->cos_mlambda[0] = 1.0f;
SphVariables->sin_mlambda[0] = 0.0; SphVariables->sin_mlambda[0] = 0.0f;
SphVariables->cos_mlambda[1] = cos_lambda; SphVariables->cos_mlambda[1] = cos_lambda;
SphVariables->sin_mlambda[1] = sin_lambda; SphVariables->sin_mlambda[1] = sin_lambda;
@ -480,9 +480,9 @@ int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint
*/ */
{ {
float sin_phi = sin(DEG2RAD(CoordSpherical->phig)); /* sin (geocentric latitude) */ float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */
if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */ if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) /* If nMax is less tha 16 or at the poles */
{ {
if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0) if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0)
return -1; // error return -1; // error
@ -508,7 +508,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
dV ^ 1 dV ^ 1 dV ^ dV ^ 1 dV ^ 1 dV ^
grad V = -- r + - -- t + -------- -- p grad V = -- r + - -- t + -------- -- p
dr r dt r sin(t) dp dr r dt r sinf(t) dp
INPUT : LegendreFunction INPUT : LegendreFunction
MagneticModel MagneticModel
@ -535,7 +535,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
index = (n * (n + 1) / 2 + m); index = (n * (n + 1) / 2 + m);
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/
MagneticResults->Bz -= MagneticResults->Bz -=
@ -545,7 +545,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
* (float)(n + 1) * LegendreFunction->Pcup[index]; * (float)(n + 1) * LegendreFunction->Pcup[index];
/* 1 nMax (n+2) n m m m /* 1 nMax (n+2) n m m m
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By += MagneticResults->By +=
@ -554,7 +554,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m]) SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m])
* (float)(m) * LegendreFunction->Pcup[index]; * (float)(m) * LegendreFunction->Pcup[index];
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */
@ -567,8 +567,8 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
} }
} }
cos_phi = cos(DEG2RAD(CoordSpherical->phig)); cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
if (fabs(cos_phi) > 1.0e-10) if (fabsf(cos_phi) > 1.0e-10f)
{ {
MagneticResults->By = MagneticResults->By / cos_phi; MagneticResults->By = MagneticResults->By / cos_phi;
} }
@ -617,7 +617,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
index = (n * (n + 1) / 2 + m); index = (n * (n + 1) / 2 + m);
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi)) Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to radius.*/ /* Derivative with respect to radius.*/
MagneticResults->Bz -= MagneticResults->Bz -=
@ -627,7 +627,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
* (float)(n + 1) * LegendreFunction->Pcup[index]; * (float)(n + 1) * LegendreFunction->Pcup[index];
/* 1 nMax (n+2) n m m m /* 1 nMax (n+2) n m m m
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to longitude, divided by radius. */ /* Derivative with respect to longitude, divided by radius. */
MagneticResults->By += MagneticResults->By +=
@ -636,7 +636,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m]) SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m])
* (float)(m) * LegendreFunction->Pcup[index]; * (float)(m) * LegendreFunction->Pcup[index];
/* nMax (n+2) n m m m /* nMax (n+2) n m m m
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to latitude, divided by radius. */ /* Derivative with respect to latitude, divided by radius. */
@ -647,8 +647,8 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
* LegendreFunction->dPcup[index]; * LegendreFunction->dPcup[index];
} }
} }
cos_phi = cos(DEG2RAD(CoordSpherical->phig)); cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
if (fabs(cos_phi) > 1.0e-10) if (fabsf(cos_phi) > 1.0e-10f)
{ {
MagneticResults->By = MagneticResults->By / cos_phi; MagneticResults->By = MagneticResults->By / cos_phi;
} }
@ -695,11 +695,11 @@ int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
*/ */
{ {
/* Difference between the spherical and Geodetic latitudes */ /* Difference between the spherical and Geodetic latitudes */
float Psi = (M_PI / 180) * (CoordSpherical->phig - CoordGeodetic->phi); float Psi = (M_PI_F / 180) * (CoordSpherical->phig - CoordGeodetic->phi);
/* Rotate spherical field components to the Geodeitic system */ /* Rotate spherical field components to the Geodeitic system */
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi); MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi);
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi); MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi);
MagneticResultsGeo->By = MagneticResultsSph->By; MagneticResultsGeo->By = MagneticResultsSph->By;
return 0; return 0;
@ -727,10 +727,10 @@ int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGe
GeoMagneticElements->Y = MagneticResultsGeo->By; GeoMagneticElements->Y = MagneticResultsGeo->By;
GeoMagneticElements->Z = MagneticResultsGeo->Bz; GeoMagneticElements->Z = MagneticResultsGeo->Bz;
GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By); GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By);
GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz); GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X)); GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X));
GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H)); GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H));
return 0; // OK return 0; // OK
} }
@ -762,10 +762,10 @@ int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, W
(MagneticElements->X * MagneticElements->Xdot + (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 = MagneticElements->Decldot =
180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot - 180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot -
MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H); MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H);
MagneticElements->Incldot = MagneticElements->Incldot =
180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot - 180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot -
MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F); MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F);
MagneticElements->GVdot = MagneticElements->Decldot; MagneticElements->GVdot = MagneticElements->Decldot;
@ -776,7 +776,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
/* This function evaluates all of the Schmidt-semi normalized associated Legendre /* This function evaluates all of the Schmidt-semi normalized associated Legendre
functions up to degree nMax. The functions are initially scaled by functions up to degree nMax. The functions are initially scaled by
10^280 sin^m in order to minimize the effects of underflow at large m 10^280 sinf^m in order to minimize the effects of underflow at large m
near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299). near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299).
Note that this function performs the same operation as WMM_PcupLow. Note that this function performs the same operation as WMM_PcupLow.
However this function also can be used for high degree (large nMax) models. However this function also can be used for high degree (large nMax) models.
@ -784,7 +784,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
Calling Parameters: Calling Parameters:
INPUT INPUT
nMax: Maximum spherical harmonic degree to compute. nMax: Maximum spherical harmonic degree to compute.
x: cos(colatitude) or sin(latitude). x: cosf(colatitude) or sinf(latitude).
OUTPUT OUTPUT
Pcup: A vector of all associated Legendgre polynomials evaluated at Pcup: A vector of all associated Legendgre polynomials evaluated at
@ -800,9 +800,9 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
Change from the previous version Change from the previous version
The prevous version computes the derivatives as The prevous version computes the derivatives as
dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ). dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ).
However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude. However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude.
Hence the derivatives are multiplied by sin(latitude). Hence the derivatives are multiplied by sinf(latitude).
Removed the options for CS phase and normalizations. Removed the options for CS phase and normalizations.
Note: In geomagnetism, the derivatives of ALF are usually found with Note: In geomagnetism, the derivatives of ALF are usually found with
@ -842,7 +842,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
scalef = 1.0e-280; scalef = 1.0e-280;
for (n = 0; n <= 2 * nMax + 1; ++n) for (n = 0; n <= 2 * nMax + 1; ++n)
PreSqr[n] = sqrt((float)(n)); PreSqr[n] = sqrtf((float)(n));
k = 2; k = 2;
@ -860,10 +860,10 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
k = k + 2; k = k + 2;
} }
/*z = sin (geocentric latitude) */ /*z = sinf (geocentric latitude) */
z = sqrt((1.0 - x) * (1.0 + x)); z = sqrtf((1.0f - x) * (1.0f + x));
pm2 = 1.0; pm2 = 1.0;
Pcup[0] = 1.0; Pcup[0] = 1.0f;
dPcup[0] = 0.0; dPcup[0] = 0.0;
if (nMax == 0) if (nMax == 0)
{ {
@ -945,7 +945,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
Calling Parameters: Calling Parameters:
INPUT INPUT
nMax: Maximum spherical harmonic degree to compute. nMax: Maximum spherical harmonic degree to compute.
x: cos(colatitude) or sin(latitude). x: cosf(colatitude) or sinf(latitude).
OUTPUT OUTPUT
Pcup: A vector of all associated Legendgre polynomials evaluated at Pcup: A vector of all associated Legendgre polynomials evaluated at
@ -975,8 +975,8 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
Pcup[0] = 1.0; Pcup[0] = 1.0;
dPcup[0] = 0.0; dPcup[0] = 0.0;
/*sin (geocentric latitude) - sin_phi */ /*sinf (geocentric latitude) - sin_phi */
z = sqrt((1.0 - x) * (1.0 + x)); z = sqrtf((1.0f - x) * (1.0f + x));
/* First, Compute the Gauss-normalized associated Legendre functions */ /* First, Compute the Gauss-normalized associated Legendre functions */
for (n = 1; n <= nMax; n++) for (n = 1; n <= nMax; n++)
@ -1033,7 +1033,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
{ {
index = (n * (n + 1) / 2 + m); index = (n * (n + 1) / 2 + m);
index1 = (n * (n + 1) / 2 + m - 1); 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] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m));
} }
} }
@ -1086,7 +1086,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
schmidtQuasiNorm1 = 1.0; schmidtQuasiNorm1 = 1.0;
MagneticResults->By = 0.0; MagneticResults->By = 0.0;
sin_phi = sin(DEG2RAD(CoordSpherical->phig)); sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
for (n = 1; n <= MagneticModel->nMax; n++) for (n = 1; n <= MagneticModel->nMax; n++)
{ {
@ -1097,7 +1097,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
index = (n * (n + 1) / 2 + 1); index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1)); schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2; schmidtQuasiNorm1 = schmidtQuasiNorm2;
if (n == 1) if (n == 1)
{ {
@ -1110,7 +1110,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
} }
/* 1 nMax (n+2) n m m m /* 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)) By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
@ -1152,13 +1152,13 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
schmidtQuasiNorm1 = 1.0; schmidtQuasiNorm1 = 1.0;
MagneticResults->By = 0.0; MagneticResults->By = 0.0;
sin_phi = sin(DEG2RAD(CoordSpherical->phig)); sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) for (n = 1; n <= MagneticModel->nMaxSecVar; n++)
{ {
index = (n * (n + 1) / 2 + 1); index = (n * (n + 1) / 2 + 1);
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n; schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1)); schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2; schmidtQuasiNorm1 = schmidtQuasiNorm2;
if (n == 1) if (n == 1)
{ {
@ -1171,7 +1171,7 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
} }
/* 1 nMax (n+2) n m m m /* 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)) By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */ n=1 m=0 n n n */
/* Derivative with respect to longitude, divided by radius. */ /* Derivative with respect to longitude, divided by radius. */
@ -1291,7 +1291,7 @@ int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year)
temp += MonthDays[i - 1]; temp += MonthDays[i - 1];
temp += day; temp += day;
decimal_date = year + (temp - 1) / (365.0 + ExtraDay); decimal_date = year + (temp - 1) / (365.0f + ExtraDay);
return 0; // OK return 0; // OK
} }
@ -1304,21 +1304,21 @@ int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_Coord
{ {
float CosLat, SinLat, rc, xp, zp; // all local variables float CosLat, SinLat, rc, xp, zp; // all local variables
CosLat = cos(DEG2RAD(CoordGeodetic->phi)); CosLat = cosf(DEG2RAD(CoordGeodetic->phi));
SinLat = sin(DEG2RAD(CoordGeodetic->phi)); SinLat = sinf(DEG2RAD(CoordGeodetic->phi));
// compute the local radius of curvature on the WGS-84 reference ellipsoid // compute the local radius of curvature on the WGS-84 reference ellipsoid
rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat); rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat);
// compute ECEF Cartesian coordinates of specified point (for longitude=0) // compute ECEF Cartesian coordinates of specified point (for longitude=0)
xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
zp = (rc * (1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
// compute spherical radius and angle lambda and phi of specified point // compute spherical radius and angle lambda and phi of specified point
CoordSpherical->r = sqrt(xp * xp + zp * zp); CoordSpherical->r = sqrtf(xp * xp + zp * zp);
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
return 0; // OK return 0; // OK

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

@ -35,8 +35,9 @@
#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); #define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2);
#define NUMPCUP 92 // NUMTERMS +1 #define NUMPCUP 92 // NUMTERMS +1
#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 #define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1
#define RAD2DEG(rad) ((rad)*(180.0L/M_PI)) #define M_PI_F ((float)M_PI)
#define DEG2RAD(deg) ((deg)*(M_PI/180.0L)) #define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F))
#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f))
// internal structure definitions // internal structure definitions
typedef struct { typedef struct {

8
flight/Libraries/math/sin_lookup.c
View File

@ -81,7 +81,7 @@ int sin_lookup_initalize()
return -1; return -1;
for(uint32_t i = 0; i < 180; i++) for(uint32_t i = 0; i < 180; i++)
sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f); sin_table[i] = sinf((float)i * 2 * ((float)M_PI) / 360.0f);
return 0; return 0;
} }
@ -126,7 +126,7 @@ float cos_lookup_deg(float angle)
*/ */
float sin_lookup_rad(float angle) float sin_lookup_rad(float angle)
{ {
int degrees = angle * 180.0f / M_PI; int degrees = angle * 180.0f / ((float)M_PI);
return sin_lookup_deg(degrees); return sin_lookup_deg(degrees);
} }
@ -137,6 +137,6 @@ float sin_lookup_rad(float angle)
*/ */
float cos_lookup_rad(float angle) float cos_lookup_rad(float angle)
{ {
int degrees = angle * 180.0f / M_PI; int degrees = angle * 180.0f / ((float)M_PI);
return cos_lookup_deg(degrees); return cos_lookup_deg(degrees);
} }

12
flight/Modules/Attitude/attitude.c
View File

@ -513,10 +513,10 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * M_PI / 180 / 2; qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (((float)M_PI) / 180 / 2);
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * M_PI / 180 / 2; qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (((float)M_PI) / 180 / 2);
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * M_PI / 180 / 2; qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (((float)M_PI) / 180 / 2);
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * M_PI / 180 / 2; qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (((float)M_PI) / 180 / 2);
// Take a time step // Take a time step
q[0] = q[0] + qdot[0]; q[0] = q[0] + qdot[0];
@ -541,7 +541,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
// If quaternion has become inappropriately short or is nan reinit. // If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN // THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) { if((fabsf(qmag) < 1e-3f) || (qmag != qmag)) {
q[0] = 1; q[0] = 1;
q[1] = 0; q[1] = 0;
q[2] = 0; q[2] = 0;
@ -571,7 +571,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module. // Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025; const float fakeDt = 0.0025;
if (attitudeSettings.AccelTau < 0.0001) { if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0 accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false; accel_filter_enabled = false;
} else { } else {

4
flight/Modules/CameraStab/camerastab.c
View File

@ -295,7 +295,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
if (index == CAMERASTABSETTINGS_INPUT_ROLL) { if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
float pitch; float pitch;
AttitudeActualPitchGet(&pitch); AttitudeActualPitchGet(&pitch);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabs(pitch)) gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH];
} }
break; break;
@ -303,7 +303,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
if (index == CAMERASTABSETTINGS_INPUT_PITCH) { if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
float roll; float roll;
AttitudeActualRollGet(&roll); AttitudeActualRollGet(&roll);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabs(roll)) gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL];
} }
break; break;

6
flight/Modules/Sensors/sensors.c
View File

@ -67,7 +67,7 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY+3) #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define SENSOR_PERIOD 2 #define SENSOR_PERIOD 2
#define F_PI 3.14159265358979323846f #define F_PI ((float)M_PI)
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI) #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
// Private types // Private types
@ -498,8 +498,8 @@ static void magOffsetEstimation(MagnetometerData *mag)
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
float cy = cosf(attitude.Yaw * M_PI / 180.0f); float cy = cosf(attitude.Yaw * F_PI / 180.0f);
float sy = sinf(attitude.Yaw * M_PI / 180.0f); float sy = sinf(attitude.Yaw * F_PI / 180.0f);
xy[0] = cy * B_e[0] + sy * B_e[1]; xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1]; xy[1] = -sy * B_e[0] + cy * B_e[1];

4
flight/Modules/Stabilization/stabilization.c
View File

@ -296,7 +296,7 @@ static void stabilizationTask(void* parameters)
if (reinit) if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0;
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { if(fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode // While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0; axis_lock_accum[i] = 0;
@ -485,7 +485,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation // calculation
const float fakeDt = 0.0025; const float fakeDt = 0.0025;
if(settings.GyroTau < 0.0001) if(settings.GyroTau < 0.0001f)
gyro_alpha = 0; // not trusting this to resolve to 0 gyro_alpha = 0; // not trusting this to resolve to 0
else else
gyro_alpha = expf(-fakeDt / settings.GyroTau); gyro_alpha = expf(-fakeDt / settings.GyroTau);

2
flight/Modules/Stabilization/virtualflybar.c
View File

@ -55,7 +55,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Command signal can indicate how much to disregard the gyro feedback (fast flips) // Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) { if (settings->VbarGyroSuppress > 0) {
gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f); gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f);
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
} }

2
flight/Modules/System/systemmod.c
View File

@ -443,7 +443,7 @@ static void updateStats()
idleCounterClear = 1; idleCounterClear = 1;
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR) #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
float temp_voltage = 3.3 * PIOS_ADC_PinGet(0) / ((1 << 12) - 1); float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((float)((1 << 12) - 1));
const float STM32_TEMP_V25 = 1.43; /* V */ const float STM32_TEMP_V25 = 1.43; /* V */
const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */ const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25; stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;