rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +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;
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 ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG * atan2f(y, x);
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
mag = VectorMagnitude(v1b);
if (fabs(mag) < 1e-30)
if (fabsf(mag) < 1e-30f)
return (-1);
for (i=0;i<3;i++)
Rib[0][i]=v1b[i]/mag;
mag = VectorMagnitude(v1e);
if (fabs(mag) < 1e-30)
if (fabsf(mag) < 1e-30f)
return (-1);
for (i=0;i<3;i++)
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
CrossProduct(v1b,v2b,&Rib[1][0]);
mag = VectorMagnitude(&Rib[1][0]);
if (fabs(mag) < 1e-30)
if (fabsf(mag) < 1e-30f)
return (-1);
for (i=0;i<3;i++)
Rib[1][i]=Rib[1][i]/mag;
CrossProduct(v1e,v2e,&Rie[1][0]);
mag = VectorMagnitude(&Rie[1][0]);
if (fabs(mag) < 1e-30)
if (fabsf(mag) < 1e-30f)
return (-1);
for (i=0;i<3;i++)
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->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
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;
}
B[0] = GeoMagneticElements->X * 1e-2;
B[1] = GeoMagneticElements->Y * 1e-2;
B[2] = GeoMagneticElements->Z * 1e-2;
B[0] = GeoMagneticElements->X * 1e-2f;
B[1] = GeoMagneticElements->Y * 1e-2f;
B[2] = GeoMagneticElements->Z * 1e-2f;
return returned;
}
@ -433,8 +433,8 @@ int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical
float cos_lambda, sin_lambda;
uint16_t m, n;
cos_lambda = cos(DEG2RAD(CoordSpherical->lambda));
sin_lambda = sin(DEG2RAD(CoordSpherical->lambda));
cos_lambda = cosf(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 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);
/*
Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax
cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b)
sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b)
Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax
cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b)
sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b)
*/
SphVariables->cos_mlambda[0] = 1.0;
SphVariables->sin_mlambda[0] = 0.0;
SphVariables->cos_mlambda[0] = 1.0f;
SphVariables->sin_mlambda[0] = 0.0f;
SphVariables->cos_mlambda[1] = cos_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)
return -1; // error
@ -508,7 +508,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
dV ^ 1 dV ^ 1 dV ^
grad V = -- r + - -- t + -------- -- p
dr r dt r sin(t) dp
dr r dt r sinf(t) dp
INPUT : LegendreFunction
MagneticModel
@ -535,7 +535,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
index = (n * (n + 1) / 2 + 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 */
/* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/
MagneticResults->Bz -=
@ -545,7 +545,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
* (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))
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
MagneticResults->By +=
@ -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])
* (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))
Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */
/* 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));
if (fabs(cos_phi) > 1.0e-10)
cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
if (fabsf(cos_phi) > 1.0e-10f)
{
MagneticResults->By = MagneticResults->By / cos_phi;
}
@ -617,7 +617,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
index = (n * (n + 1) / 2 + 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 */
/* Derivative with respect to radius.*/
MagneticResults->Bz -=
@ -627,7 +627,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
* (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))
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */
/* Derivative with respect to longitude, divided by radius. */
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])
* (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))
Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
n=1 m=0 n n n */
/* Derivative with respect to latitude, divided by radius. */
@ -647,8 +647,8 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
* LegendreFunction->dPcup[index];
}
}
cos_phi = cos(DEG2RAD(CoordSpherical->phig));
if (fabs(cos_phi) > 1.0e-10)
cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
if (fabsf(cos_phi) > 1.0e-10f)
{
MagneticResults->By = MagneticResults->By / cos_phi;
}
@ -695,11 +695,11 @@ int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
*/
{
/* 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 */
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi);
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi);
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi);
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi);
MagneticResultsGeo->By = MagneticResultsSph->By;
return 0;
@ -727,10 +727,10 @@ int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGe
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 = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By);
GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X));
GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H));
return 0; // OK
}
@ -762,10 +762,10 @@ int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, W
(MagneticElements->X * MagneticElements->Xdot +
MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F;
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->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->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
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).
Note that this function performs the same operation as WMM_PcupLow.
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:
INPUT
nMax: Maximum spherical harmonic degree to compute.
x: cos(colatitude) or sin(latitude).
x: cosf(colatitude) or sinf(latitude).
OUTPUT
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
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.
Hence the derivatives are multiplied by sin(latitude).
Hence the derivatives are multiplied by sinf(latitude).
Removed the options for CS phase and normalizations.
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;
for (n = 0; n <= 2 * nMax + 1; ++n)
PreSqr[n] = sqrt((float)(n));
PreSqr[n] = sqrtf((float)(n));
k = 2;
@ -860,10 +860,10 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
k = k + 2;
}
/*z = sin (geocentric latitude) */
z = sqrt((1.0 - x) * (1.0 + x));
/*z = sinf (geocentric latitude) */
z = sqrtf((1.0f - x) * (1.0f + x));
pm2 = 1.0;
Pcup[0] = 1.0;
Pcup[0] = 1.0f;
dPcup[0] = 0.0;
if (nMax == 0)
{
@ -945,7 +945,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
Calling Parameters:
INPUT
nMax: Maximum spherical harmonic degree to compute.
x: cos(colatitude) or sin(latitude).
x: cosf(colatitude) or sinf(latitude).
OUTPUT
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;
dPcup[0] = 0.0;
/*sin (geocentric latitude) - sin_phi */
z = sqrt((1.0 - x) * (1.0 + x));
/*sinf (geocentric latitude) - sin_phi */
z = sqrtf((1.0f - x) * (1.0f + x));
/* First, Compute the Gauss-normalized associated Legendre functions */
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);
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;
MagneticResults->By = 0.0;
sin_phi = sin(DEG2RAD(CoordSpherical->phig));
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
for (n = 1; n <= MagneticModel->nMax; n++)
{
@ -1097,7 +1097,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
index = (n * (n + 1) / 2 + 1);
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;
if (n == 1)
{
@ -1110,7 +1110,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
}
/* 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 */
/* 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;
MagneticResults->By = 0.0;
sin_phi = sin(DEG2RAD(CoordSpherical->phig));
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
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));
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1));
schmidtQuasiNorm1 = schmidtQuasiNorm2;
if (n == 1)
{
@ -1171,7 +1171,7 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
}
/* 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 */
/* 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 += day;
decimal_date = year + (temp - 1) / (365.0 + ExtraDay);
decimal_date = year + (temp - 1) / (365.0f + ExtraDay);
return 0; // OK
}
@ -1304,21 +1304,21 @@ int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_Coord
{
float CosLat, SinLat, rc, xp, zp; // all local variables
CosLat = cos(DEG2RAD(CoordGeodetic->phi));
SinLat = sin(DEG2RAD(CoordGeodetic->phi));
CosLat = cosf(DEG2RAD(CoordGeodetic->phi));
SinLat = sinf(DEG2RAD(CoordGeodetic->phi));
// compute the local radius of curvature on the WGS-84 reference ellipsoid
rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat);
rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat);
// compute ECEF Cartesian coordinates of specified point (for longitude=0)
xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
zp = (rc * (1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
// compute spherical radius and angle lambda and phi of specified point
CoordSpherical->r = sqrt(xp * xp + zp * zp);
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude
CoordSpherical->r = sqrtf(xp * xp + zp * zp);
CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
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 NUMPCUP 92 // NUMTERMS +1
#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1
#define RAD2DEG(rad) ((rad)*(180.0L/M_PI))
#define DEG2RAD(deg) ((deg)*(M_PI/180.0L))
#define M_PI_F ((float)M_PI)
#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F))
#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f))
// internal structure definitions
typedef struct {

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

@ -81,7 +81,7 @@ int sin_lookup_initalize()
return -1;
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;
}
@ -126,7 +126,7 @@ float cos_lookup_deg(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);
}
@ -137,6 +137,6 @@ float sin_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);
}
}

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
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * M_PI / 180 / 2;
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * M_PI / 180 / 2;
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * M_PI / 180 / 2;
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * 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 * (((float)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 * (((float)M_PI) / 180 / 2);
// Take a time step
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.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
if((fabsf(qmag) < 1e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 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.
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_filter_enabled = false;
} 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) {
float pitch;
AttitudeActualPitchGet(&pitch);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabs(pitch))
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH];
}
break;
@ -303,7 +303,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
float roll;
AttitudeActualRollGet(&roll);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabs(roll))
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL];
}
break;

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

@ -67,7 +67,7 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#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)
// 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[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 sy = sinf(attitude.Yaw * M_PI / 180.0f);
float cy = cosf(attitude.Yaw * F_PI / 180.0f);
float sy = sinf(attitude.Yaw * F_PI / 180.0f);
xy[0] = cy * B_e[0] + sy * 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)
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
rateDesiredAxis[i] = attitudeDesiredAxis[i];
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
// calculation
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
else
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)
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;
}

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

@ -443,7 +443,7 @@ static void updateStats()
idleCounterClear = 1;
#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_AVG_SLOPE = 4.3; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;