rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-931: Makes flight code compile with -Wfloat-equal and -Wunsuffixed-float-constants enabled.

Browse Source 
Also fixes warnings (and bugs) in F4 STM32_USB_OTG_Driver code, allowing -Werror to be enabled for all flight code.
Fixes all other compiler warnings that would otherwise cause the flight code to not compile with -Werror enabled.
Along the way, this also adds some uses of isnan() to various places rather than questionable tests for x != x and
x == x to check for NaNs.

+review OPReview
This commit is contained in:
Richard Flay (Hyper) 2013-04-30 20:36:42 +09:30
parent 80c917591e
commit 7937ae6296
33 changed files with 291 additions and 281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/libraries/CoordinateConversions.c
View File

@ -37,8 +37,8 @@
// ****** convert Lat,Lon,Alt to ECEF ************ // ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3]) void LLA2ECEF(float LLA[3], float ECEF[3])
{ {
const float a = 6378137.0; // Equatorial Radius const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2; // Eccentricity const float e = 8.1819190842622e-2f; // Eccentricity
float sinLat, sinLon, cosLat, cosLon; float sinLat, sinLon, cosLat, cosLon;
float N; float N;
@ -66,7 +66,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
* Suggestion: [0,0,0] * Suggestion: [0,0,0]
**/ **/
const float a = 6378137.0; // Equatorial Radius const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2f; // Eccentricity const float e = 8.1819190842622e-2f; // Eccentricity
float x = ECEF[0], y = ECEF[1], z = ECEF[2]; float x = ECEF[0], y = ECEF[1], z = ECEF[2];
float Lat, N, NplusH, delta, esLat; float Lat, N, NplusH, delta, esLat;

240
flight/libraries/WorldMagModel.c
View File

@ -59,97 +59,97 @@
// http://reviews.openpilot.org/cru/OPReview-436#c6476 : // http://reviews.openpilot.org/cru/OPReview-436#c6476 :
// first column not used but it will be optimized out by compiler // first column not used but it will be optimized out by compiler
static const float CoeffFile[91][6] = { {0, 0, 0, 0, 0, 0}, static const float CoeffFile[91][6] = { {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{1, 0, -29496.6, 0.0, 11.6, 0.0}, {1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f},
{1, 1, -1586.3, 4944.4, 16.5, -25.9}, {1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f},
{2, 0, -2396.6, 0.0, -12.1, 0.0}, {2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f},
{2, 1, 3026.1, -2707.7, -4.4, -22.5}, {2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f},
{2, 2, 1668.6, -576.1, 1.9, -11.8}, {2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f},
{3, 0, 1340.1, 0.0, 0.4, 0.0}, {3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f},
{3, 1, -2326.2, -160.2, -4.1, 7.3}, {3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f},
{3, 2, 1231.9, 251.9, -2.9, -3.9}, {3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f},
{3, 3, 634.0, -536.6, -7.7, -2.6}, {3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f},
{4, 0, 912.6, 0.0, -1.8, 0.0}, {4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f},
{4, 1, 808.9, 286.4, 2.3, 1.1}, {4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f},
{4, 2, 166.7, -211.2, -8.7, 2.7}, {4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f},
{4, 3, -357.1, 164.3, 4.6, 3.9}, {4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f},
{4, 4, 89.4, -309.1, -2.1, -0.8}, {4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f},
{5, 0, -230.9, 0.0, -1.0, 0.0}, {5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f},
{5, 1, 357.2, 44.6, 0.6, 0.4}, {5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f},
{5, 2, 200.3, 188.9, -1.8, 1.8}, {5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f},
{5, 3, -141.1, -118.2, -1.0, 1.2}, {5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f},
{5, 4, -163.0, 0.0, 0.9, 4.0}, {5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f},
{5, 5, -7.8, 100.9, 1.0, -0.6}, {5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f},
{6, 0, 72.8, 0.0, -0.2, 0.0}, {6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f},
{6, 1, 68.6, -20.8, -0.2, -0.2}, {6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f},
{6, 2, 76.0, 44.1, -0.1, -2.1}, {6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f},
{6, 3, -141.4, 61.5, 2.0, -0.4}, {6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f},
{6, 4, -22.8, -66.3, -1.7, -0.6}, {6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f},
{6, 5, 13.2, 3.1, -0.3, 0.5}, {6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f},
{6, 6, -77.9, 55.0, 1.7, 0.9}, {6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f},
{7, 0, 80.5, 0.0, 0.1, 0.0}, {7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f},
{7, 1, -75.1, -57.9, -0.1, 0.7}, {7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f},
{7, 2, -4.7, -21.1, -0.6, 0.3}, {7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f},
{7, 3, 45.3, 6.5, 1.3, -0.1}, {7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f},
{7, 4, 13.9, 24.9, 0.4, -0.1}, {7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f},
{7, 5, 10.4, 7.0, 0.3, -0.8}, {7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f},
{7, 6, 1.7, -27.7, -0.7, -0.3}, {7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f},
{7, 7, 4.9, -3.3, 0.6, 0.3}, {7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f},
{8, 0, 24.4, 0.0, -0.1, 0.0}, {8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f},
{8, 1, 8.1, 11.0, 0.1, -0.1}, {8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f},
{8, 2, -14.5, -20.0, -0.6, 0.2}, {8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f},
{8, 3, -5.6, 11.9, 0.2, 0.4}, {8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f},
{8, 4, -19.3, -17.4, -0.2, 0.4}, {8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f},
{8, 5, 11.5, 16.7, 0.3, 0.1}, {8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f},
{8, 6, 10.9, 7.0, 0.3, -0.1}, {8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f},
{8, 7, -14.1, -10.8, -0.6, 0.4}, {8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f},
{8, 8, -3.7, 1.7, 0.2, 0.3}, {8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f},
{9, 0, 5.4, 0.0, 0.0, 0.0}, {9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f},
{9, 1, 9.4, -20.5, -0.1, 0.0}, {9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f},
{9, 2, 3.4, 11.5, 0.0, -0.2}, {9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f},
{9, 3, -5.2, 12.8, 0.3, 0.0}, {9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f},
{9, 4, 3.1, -7.2, -0.4, -0.1}, {9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f},
{9, 5, -12.4, -7.4, -0.3, 0.1}, {9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f},
{9, 6, -0.7, 8.0, 0.1, 0.0}, {9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f},
{9, 7, 8.4, 2.1, -0.1, -0.2}, {9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f},
{9, 8, -8.5, -6.1, -0.4, 0.3}, {9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f},
{9, 9, -10.1, 7.0, -0.2, 0.2}, {9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f},
{10, 0, -2.0, 0.0, 0.0, 0.0}, {10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f},
{10, 1, -6.3, 2.8, 0.0, 0.1}, {10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f},
{10, 2, 0.9, -0.1, -0.1, -0.1}, {10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f},
{10, 3, -1.1, 4.7, 0.2, 0.0}, {10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f},
{10, 4, -0.2, 4.4, 0.0, -0.1}, {10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f},
{10, 5, 2.5, -7.2, -0.1, -0.1}, {10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f},
{10, 6, -0.3, -1.0, -0.2, 0.0}, {10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f},
{10, 7, 2.2, -3.9, 0.0, -0.1}, {10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f},
{10, 8, 3.1, -2.0, -0.1, -0.2}, {10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f},
{10, 9, -1.0, -2.0, -0.2, 0.0}, {10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f},
{10, 10, -2.8, -8.3, -0.2, -0.1}, {10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f},
{11, 0, 3.0, 0.0, 0.0, 0.0}, {11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f},
{11, 1, -1.5, 0.2, 0.0, 0.0}, {11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f},
{11, 2, -2.1, 1.7, 0.0, 0.1}, {11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f},
{11, 3, 1.7, -0.6, 0.1, 0.0}, {11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f},
{11, 4, -0.5, -1.8, 0.0, 0.1}, {11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f},
{11, 5, 0.5, 0.9, 0.0, 0.0}, {11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f},
{11, 6, -0.8, -0.4, 0.0, 0.1}, {11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f},
{11, 7, 0.4, -2.5, 0.0, 0.0}, {11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f},
{11, 8, 1.8, -1.3, 0.0, -0.1}, {11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f},
{11, 9, 0.1, -2.1, 0.0, -0.1}, {11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f},
{11, 10, 0.7, -1.9, -0.1, 0.0}, {11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f},
{11, 11, 3.8, -1.8, 0.0, -0.1}, {11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f},
{12, 0, -2.2, 0.0, 0.0, 0.0}, {12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f},
{12, 1, -0.2, -0.9, 0.0, 0.0}, {12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f},
{12, 2, 0.3, 0.3, 0.1, 0.0}, {12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f},
{12, 3, 1.0, 2.1, 0.1, 0.0}, {12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f},
{12, 4, -0.6, -2.5, -0.1, 0.0}, {12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f},
{12, 5, 0.9, 0.5, 0.0, 0.0}, {12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f},
{12, 6, -0.1, 0.6, 0.0, 0.1}, {12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f},
{12, 7, 0.5, 0.0, 0.0, 0.0}, {12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f},
{12, 8, -0.4, 0.1, 0.0, 0.0}, {12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f},
{12, 9, -0.4, 0.3, 0.0, 0.0}, {12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f},
{12, 10, 0.2, -0.9, 0.0, 0.0}, {12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f},
{12, 11, -0.8, -0.2, -0.1, 0.0}, {12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f},
{12, 12, 0.0, 0.9, 0.1, 0.0} {12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f}
}; };
static WMMtype_Ellipsoid *Ellip = NULL; static WMMtype_Ellipsoid *Ellip = NULL;
@ -175,12 +175,12 @@ int WMM_Initialize()
if (!MagneticModel) return -2; // invalid pointer if (!MagneticModel) return -2; // invalid pointer
// Sets WGS-84 parameters // Sets WGS-84 parameters
Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km
Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km
Ellip->fla = 1 / 298.257223563; // flattening Ellip->fla = 1.0f / 298.257223563f; // flattening
Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity
Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared
Ellip->re = 6371.2; // Earth's radius in km Ellip->re = 6371.2f; // Earth's radius in km
// Sets Magnetic Model parameters // Sets Magnetic Model parameters
MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; MagneticModel->nMax = WMM_MAX_MODEL_DEGREES;
@ -188,15 +188,15 @@ int WMM_Initialize()
MagneticModel->SecularVariationUsed = 0; MagneticModel->SecularVariationUsed = 0;
// Really, Really needs to be read from a file - out of date in 2015 at latest // Really, Really needs to be read from a file - out of date in 2015 at latest
MagneticModel->EditionDate = 5.7863328170559505e-307; MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */
MagneticModel->epoch = 2010.0; MagneticModel->epoch = 2010.0f;
sprintf(MagneticModel->ModelName, "WMM-2010"); sprintf(MagneticModel->ModelName, "WMM-2010");
return 0; // OK return 0; // OK
} }
int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3]) int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, uint16_t Day, uint16_t Year, float B[3])
{ {
// return '0' if all appears to be OK // return '0' if all appears to be OK
// return < 0 if error // return < 0 if error
@ -245,7 +245,7 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
CoordGeodetic->phi = Lat; CoordGeodetic->phi = Lat;
CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km
// Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report // Convert from geodetic to Spherical Equations: 17-18, WMM Technical report
if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0) if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0)
returned = -7; // error returned = -7; // error
} }
@ -525,9 +525,9 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
uint16_t m, n, index; uint16_t m, n, index;
float cos_phi; float cos_phi;
MagneticResults->Bz = 0.0; MagneticResults->Bz = 0.0f;
MagneticResults->By = 0.0; MagneticResults->By = 0.0f;
MagneticResults->Bx = 0.0; MagneticResults->Bx = 0.0f;
for (n = 1; n <= MagneticModel->nMax; n++) for (n = 1; n <= MagneticModel->nMax; n++)
{ {
@ -607,9 +607,9 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
MagneticModel->SecularVariationUsed = TRUE; MagneticModel->SecularVariationUsed = TRUE;
MagneticResults->Bz = 0.0; MagneticResults->Bz = 0.0f;
MagneticResults->By = 0.0; MagneticResults->By = 0.0f;
MagneticResults->Bx = 0.0; MagneticResults->Bx = 0.0f;
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) for (n = 1; n <= MagneticModel->nMaxSecVar; n++)
{ {
@ -830,7 +830,11 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
return -1; return -1;
} }
if (fabs(x) == 1.0) /*
* Note: OP code change to avoid floating point equality test.
* Was: if (fabs(x) == 1.0)
*/
if (fabs(x) - 1.0f < 1e-9f)
{ {
FREE(PreSqr); FREE(PreSqr);
FREE(f2); FREE(f2);
@ -840,7 +844,9 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
return -2; return -2;
} }
scalef = 1.0e-280; /* OP Change: 1.0e-280 is too small to store in a float - the compiler truncates
* it to 0.0f, which is bad as the code below divides by scalef. */
scalef = 1.0e-20f;
for (n = 0; n <= 2 * nMax + 1; ++n) for (n = 0; n <= 2 * nMax + 1; ++n)
PreSqr[n] = sqrtf((float)(n)); PreSqr[n] = sqrtf((float)(n));
@ -863,9 +869,9 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
/*z = sinf (geocentric latitude) */ /*z = sinf (geocentric latitude) */
z = sqrtf((1.0f - x) * (1.0f + x)); z = sqrtf((1.0f - x) * (1.0f + x));
pm2 = 1.0; pm2 = 1.0f;
Pcup[0] = 1.0f; Pcup[0] = 1.0f;
dPcup[0] = 0.0; dPcup[0] = 0.0f;
if (nMax == 0) if (nMax == 0)
{ {
FREE(PreSqr); FREE(PreSqr);
@ -889,7 +895,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
} }
pmm = PreSqr[2] * scalef; pmm = PreSqr[2] * scalef;
rescalem = 1.0 / scalef; rescalem = 1.0f / scalef;
kstart = 0; kstart = 0;
for (m = 1; m <= nMax - 1; ++m) for (m = 1; m <= nMax - 1; ++m)
@ -973,8 +979,8 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
return -1; return -1;
} }
Pcup[0] = 1.0; Pcup[0] = 1.0f;
dPcup[0] = 0.0; dPcup[0] = 0.0f;
/*sinf (geocentric latitude) - sin_phi */ /*sinf (geocentric latitude) - sin_phi */
z = sqrtf((1.0f - x) * (1.0f + x)); z = sqrtf((1.0f - x) * (1.0f + x));
@ -1022,7 +1028,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
functions and the Schmidt quasi-normalized version. This is equivalent to functions and the Schmidt quasi-normalized version. This is equivalent to
sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */ sqrt((m==0?1:2)*(n-m)!/(n+m!))*(2n-1)!!/(n-m)! */
schmidtQuasiNorm[0] = 1.0; schmidtQuasiNorm[0] = 1.0f;
for (n = 1; n <= nMax; n++) for (n = 1; n <= nMax; n++)
{ {
index = (n * (n + 1) / 2); index = (n * (n + 1) / 2);
@ -1084,9 +1090,9 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
return -1; // memory allocation error return -1; // memory allocation error
PcupS[0] = 1; PcupS[0] = 1;
schmidtQuasiNorm1 = 1.0; schmidtQuasiNorm1 = 1.0f;
MagneticResults->By = 0.0; MagneticResults->By = 0.0f;
sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
for (n = 1; n <= MagneticModel->nMax; n++) for (n = 1; n <= MagneticModel->nMax; n++)
@ -1150,9 +1156,9 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
return -1; // memory allocation error return -1; // memory allocation error
PcupS[0] = 1; PcupS[0] = 1;
schmidtQuasiNorm1 = 1.0; schmidtQuasiNorm1 = 1.0f;
MagneticResults->By = 0.0; MagneticResults->By = 0.0f;
sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
for (n = 1; n <= MagneticModel->nMaxSecVar; n++) for (n = 1; n <= MagneticModel->nMaxSecVar; n++)

6
flight/libraries/math/pid.c
View File

@ -39,7 +39,7 @@ static float bound(float val, float range);
static float deriv_tau = 7.9577e-3f; static float deriv_tau = 7.9577e-3f;
//! Store the setpoint weight to apply for the derivative term //! Store the setpoint weight to apply for the derivative term
static float deriv_gamma = 1.0; static float deriv_gamma = 1.0f;
/** /**
* Update the PID computation * Update the PID computation
@ -58,7 +58,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
float dterm = 0; float dterm = 0;
pid->lastErr = err; pid->lastErr = err;
if(pid->d && dT) if(pid->d > 0.0f && dT > 0.0f)
{ {
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
@ -90,7 +90,7 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas
float dterm = 0; float dterm = 0;
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured); pid->lastErr = (deriv_gamma * setpoint - measured);
if(pid->d && dT) if(pid->d > 0.0f && dT > 0.0f)
{ {
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)

22
flight/libraries/printf2.c
View File

@ -178,7 +178,7 @@ int _read(int file, char *ptr, int len)
/*============================================================================== /*==============================================================================
* Write to a file. libc subroutines will use this system routine for output to * Write to a file. libc subroutines will use this system routine for output to
* all files, including stdout—so if you need to generate any output, for * all files, including stdout<EFBFBD>so if you need to generate any output, for
* example to a serial port for debugging, you should make your minimal write * example to a serial port for debugging, you should make your minimal write
* capable of doing this. * capable of doing this.
*/ */
@ -287,14 +287,14 @@ static uint my_strlen(char *str)
// It is more useful when implementing a walking-string function. // It is more useful when implementing a walking-string function.
//**************************************************************************** //****************************************************************************
static const double round_nums[8] = { static const double round_nums[8] = {
0.5, 0.5L,
0.05, 0.05L,
0.005, 0.005L,
0.0005, 0.0005L,
0.00005, 0.00005L,
0.000005, 0.000005L,
0.0000005, 0.0000005L,
0.00000005 0.00000005L
} ; } ;
static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits) static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
@ -305,9 +305,9 @@ static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
//******************************************* //*******************************************
// extract negative info // extract negative info
//******************************************* //*******************************************
if (dbl < 0.0) { if (dbl < 0.0L) {
*output++ = '-' ; *output++ = '-' ;
dbl *= -1.0 ; dbl *= -1.0L ;
} else { } else {
if (use_leading_plus) { if (use_leading_plus) {
*output++ = '+' ; *output++ = '+' ;

2
flight/modules/Actuator/actuator.c
View File

@ -431,7 +431,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
lastResult[index] = result; lastResult[index] = result;
result += accumulator; result += accumulator;
if(period !=0) if(period > 0.0f)
{ {
if(accumulator > 0.0f) if(accumulator > 0.0f)
{ {

9
flight/modules/AltitudeHold/altitudehold.c
View File

@ -177,10 +177,13 @@ static void altitudeHoldTask(void *parameters)
static float z[4] = {0, 0, 0, 0}; static float z[4] = {0, 0, 0, 0};
float z_new[4]; float z_new[4];
float P[4][4], K[4][2], x[2]; float P[4][4], K[4][2], x[2];
float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7}; float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f};
static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}}; static float V[4][4] = {{10.0f, 0.0f, 0.0f, 0.0f},
{0.0f, 100.0f, 0.0f, 0.0f},
{0.0f, 0.0f, 100.0f, 0.0f},
{0.0f, 0.0f, 0.0f, 1000.0f}};
static uint32_t accel_downsample_count = 0; static uint32_t accel_downsample_count = 0;
static float accels_accum[3] = {0,0,0}; static float accels_accum[3] = {0.0f, 0.0f, 0.0f};
float dT; float dT;
static float S[2] = {1.0f,10.0f}; static float S[2] = {1.0f,10.0f};

29
flight/modules/Attitude/attitude.c
View File

@ -92,7 +92,7 @@ static bool accel_filter_enabled = false;
static float accels_filtered[3]; static float accels_filtered[3];
static float grot_filtered[3]; static float grot_filtered[3];
static float yawBiasRate = 0; static float yawBiasRate = 0;
static float gyroGain = 0.42; static float gyroGain = 0.42f;
static int16_t accelbias[3]; static int16_t accelbias[3];
static float q[4] = {1,0,0,0}; static float q[4] = {1,0,0,0};
static float R[3][3]; static float R[3][3];
@ -217,16 +217,16 @@ static void AttitudeTask(void *parameters)
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias // For first 7 seconds use accels to get gyro bias
accelKp = 1; accelKp = 1.0f;
accelKi = 0.9; accelKi = 0.9f;
yawBiasRate = 0.01; yawBiasRate = 0.01f;
accel_filter_enabled = false; accel_filter_enabled = false;
init = 0; init = 0;
} }
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1; accelKp = 1.0f;
accelKi = 0.9; accelKi = 0.9f;
yawBiasRate = 0.01; yawBiasRate = 0.01f;
accel_filter_enabled = false; accel_filter_enabled = false;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
@ -457,7 +457,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0; static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f; dT = (thisSysTime == lastSysTime) ? 0.001f : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory // Bad practice to assume structure order, but saves memory
@ -535,18 +535,19 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
// Renomalize // Renomalize
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// 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((fabsf(qmag) < 1e-3f) || (qmag != qmag)) { if((fabsf(qmag) < 1e-3f) || isnan(qmag)) {
q[0] = 1; q[0] = 1;
q[1] = 0; q[1] = 0;
q[2] = 0; q[2] = 0;
q[3] = 0; q[3] = 0;
} else {
q[0] = q[0]/qmag;
q[1] = q[1]/qmag;
q[2] = q[2]/qmag;
q[3] = q[3]/qmag;
} }
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
@ -571,7 +572,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
gyroGain = attitudeSettings.GyroGain; gyroGain = attitudeSettings.GyroGain;
// 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.0025f;
if (attitudeSettings.AccelTau < 0.0001f) { 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;

20
flight/modules/Attitude/revolution/attitude.c
View File

@ -315,14 +315,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias // For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9; attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23; attitudeSettings.YawBiasRate = 0.23f;
magKp = 1; magKp = 1;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9; attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23; attitudeSettings.YawBiasRate = 0.23f;
magKp = 1; magKp = 1;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
@ -373,7 +373,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
MagnetometerGet(&mag); MagnetometerGet(&mag);
// If the mag is producing bad data don't use it (normally bad calibration) // If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) { if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) {
rot_mult(Rbe, homeLocation.Be, brot); rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
@ -439,7 +439,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
// 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) < 1.0e-3f) || (qmag != qmag)) { if((fabs(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1; q[0] = 1;
q[1] = 0; q[1] = 0;
q[2] = 0; q[2] = 0;
@ -601,10 +601,10 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
// Discard mag if it has NAN (normally from bad calibration) // Discard mag if it has NAN (normally from bad calibration)
mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z); mag_updated &= (!isnan(magData.x) && !isinf(magData.x) && !isnan(magData.y) && !isinf(magData.y) && !isnan(magData.z) && !isinf(magData.z));
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false) // switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); mag_updated &= (homeLocation.Be[0] > 0.0f || homeLocation.Be[1] > 0.0f || homeLocation.Be[2] > 0.0f);
// Discard airspeed if sensor not connected // Discard airspeed if sensor not connected
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ); airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );

4
flight/modules/Extensions/MagBaro/magbaro.c
View File

@ -176,7 +176,7 @@ static void magbaroTask(void *parameters)
alt_ds_count = 0; alt_ds_count = 0;
// Convert from 1/10ths of degC to degC // Convert from 1/10ths of degC to degC
data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); data.Temperature = alt_ds_temp / (10.0f * alt_ds_size);
alt_ds_temp = 0; alt_ds_temp = 0;
// Convert from Pa to kPa // Convert from Pa to kPa
@ -184,7 +184,7 @@ static void magbaroTask(void *parameters)
alt_ds_pres = 0; alt_ds_pres = 0;
// Compute the current altitude (all pressures in kPa) // Compute the current altitude (all pressures in kPa)
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f)));
// Update the AltitudeActual UAVObject // Update the AltitudeActual UAVObject
BaroAltitudeSet(&data); BaroAltitudeSet(&data);

40
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -508,25 +508,19 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane cannot hold altitude at current speed. // Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if ( if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum velocityActual.Down > 0 && // we ARE going down
&& velocityActual.Down > 0 // we ARE going down descentspeedDesired < 0 && // we WANT to go up
&& descentspeedDesired < 0 // we WANT to go up airspeedError > 0) { // we are too slow already
&& airspeedError > 0 // we are too slow already
)
{
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0; result = 0;
} }
// Error condition: plane keeps climbing despite minimum throttle (opposite of above) // Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if ( if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum velocityActual.Down < 0 && // we ARE going up
&& velocityActual.Down < 0 // we ARE going up descentspeedDesired > 0 && // we WANT to go down
&& descentspeedDesired > 0 // we WANT to go down airspeedError < 0 ) { // we are too fast already
&& airspeedError < 0 // we are too fast already
)
{
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0; result = 0;
} }
@ -558,24 +552,20 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand,
pitchCommand, fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
// Error condition: high speed dive // Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if ( if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up velocityActual.Down > 0 && // we ARE going down
&& velocityActual.Down > 0 // we ARE going down descentspeedDesired < 0 && // we WANT to go up
&& descentspeedDesired < 0 // we WANT to go up airspeedError < 0 ) { // we are too fast already
&& airspeedError < 0 // we are too fast already
) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0; result = 0;
} }
/** /**
* Compute desired roll command * Compute desired roll command
*/ */

38
flight/modules/GPS/GPS.c
View File

@ -90,7 +90,9 @@ static char* gps_rx_buffer;
static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs; static uint32_t timeOfLastUpdateMs;
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
static struct GPS_RX_STATS gpsRxStats; static struct GPS_RX_STATS gpsRxStats;
#endif
// **************** // ****************
/** /**
@ -250,7 +252,7 @@ static void gpsTask(void *parameters)
// we appear to be receiving GPS sentences OK, we've had an update // we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post... //criteria for GPS-OK taken from this post...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpsposition.PDOP < 3.5) && (gpsposition.Satellites >= 7) && if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS); AlarmsClear(SYSTEMALARMS_ALARM_GPS);
#ifdef PIOS_GPS_SETS_HOMELOCATION #ifdef PIOS_GPS_SETS_HOMELOCATION
@ -275,14 +277,14 @@ static void gpsTask(void *parameters)
*/ */
static float GravityAccel(float latitude, float longitude, float altitude) static float GravityAccel(float latitude, float longitude, float altitude)
{ {
// WGS84 gravity model. The effect of gravity over latitude is strong /* WGS84 gravity model. The effect of gravity over latitude is strong
// enough to change the estimated accelerometer bias in those apps. * enough to change the estimated accelerometer bias in those apps. */
double sinsq = sin((double)latitude); float sinsq = sinf(latitude);
sinsq *= sinsq; sinsq *= sinsq;
// Likewise, over the altitude range of a high-altitude balloon, the effect /* Likewise, over the altitude range of a high-altitude balloon, the effect
// due to change in altitude can also affect the model. * due to change in altitude can also affect the model. */
return (float)(9.7803267714 * (1 + 0.00193185138639*sinsq) / sqrt(1 - 0.00669437999013*sinsq) return (9.7803267714f * (1.0f + 0.00193185138639f*sinsq) / sqrtf(1.0f - 0.00669437999013f*sinsq)
- 3.086e-6*altitude); - 3.086e-6f*altitude);
} }
// **************** // ****************
@ -296,21 +298,23 @@ static void setHomeLocation(GPSPositionData * gpsData)
if (gps.Year >= 2000) if (gps.Year >= 2000)
{ {
// Store LLA /* Store LLA */
home.Latitude = gpsData->Latitude; home.Latitude = gpsData->Latitude;
home.Longitude = gpsData->Longitude; home.Longitude = gpsData->Longitude;
home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation;
// Compute home ECEF coordinates and the rotation matrix into NED /* Compute home ECEF coordinates and the rotation matrix into NED
double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) }; * Note that floats are used here - they should give enough precision
* for this application.*/
// Compute magnetic flux direction at home location float LLA[3] = { (home.Latitude)/10e6f, (home.Longitude)/10e6f, (home.Altitude) };
if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0)
{ // calculations appeared to go OK
// Compute local acceleration due to gravity. Vehicles that span a very large /* Compute magnetic flux direction at home location */
// range of altitude (say, weather balloons) may need to update this during the if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) {
// flight.
/*Compute local acceleration due to gravity. Vehicles that span a very large
* range of altitude (say, weather balloons) may need to update this during the
* flight. */
home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]);
home.Set = HOMELOCATION_SET_TRUE; home.Set = HOMELOCATION_SET_TRUE;
HomeLocationSet(&home); HomeLocationSet(&home);

21
flight/modules/ManualControl/manualcontrol.c
View File

@ -353,7 +353,7 @@ static void manualControlTask(void *parameters)
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs // Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband) { if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband); applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband); applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband); applyDeadband(&cmd.Yaw, settings.Deadband);
@ -696,11 +696,6 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
*/ */
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home) static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
{ {
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
lastSysTime = thisSysTime;
if (home && changed) { if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position // Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual; PositionActualData positionActual;
@ -754,8 +749,8 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
*/ */
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{ {
const float DEADBAND_HIGH = 0.55; const float DEADBAND_HIGH = 0.55f;
const float DEADBAND_LOW = 0.45; const float DEADBAND_LOW = 0.45f;
static portTickType lastSysTime; static portTickType lastSysTime;
static bool zeroed = false; static bool zeroed = false;
@ -768,7 +763,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f);
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
@ -829,9 +824,11 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
} }
// Bound // Bound
if (valueScaled > 1.0) valueScaled = 1.0; if (valueScaled > 1.0f) {
else valueScaled = 1.0f;
if (valueScaled < -1.0) valueScaled = -1.0; } else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled; return valueScaled;
} }

1
flight/modules/Osd/WavPlayer/wavplayer.c
View File

@ -80,7 +80,6 @@ MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart)
static void WavPlayerTask(void *parameters) static void WavPlayerTask(void *parameters)
{ {
portTickType xDelay = 100 / portTICK_RATE_MS;
portTickType lastSysTime; portTickType lastSysTime;
// Loop forever // Loop forever
lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS; lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS;

24
flight/modules/Osd/osdgen/osdgen.c
View File

@ -292,7 +292,7 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
plotFourQuadrants(centerX, centerY, x, y); plotFourQuadrants(centerX, centerY, x, y);
} }
error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
while (y>=0) while (y>=0)
{ {
@ -1908,13 +1908,13 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
int16_t x0 = (size/2)-dx; int16_t x0 = (size/2)-dx;
int16_t y0 = (size/2)+dy; int16_t y0 = (size/2)+dy;
// calculate the line function // calculate the line function
if((angle != 90) && (angle != -90)) if((angle < 90.0f) && (angle > -90.0f))
{ {
k = tanf(alpha);
vertical = 0; vertical = 0;
if(k==0) if(fabsf(angle) < 1e-5f) {
{ horizontal = 1;
horizontal=1; } else {
k = tanf(alpha);
} }
} }
else else
@ -1960,7 +1960,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
// horizon line // horizon line
write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1); write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
//fill //fill
if(angle<=0 && angle>-90) if(angle <= 0.0f && angle > -90.0f)
{ {
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y2;i<size;i++) for(int i=y2;i<size;i++)
@ -1973,7 +1973,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1); write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1);
} }
} }
else if(angle<-90) else if(angle < -90.0f)
{ {
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y2;i++) for(int i=0;i<y2;i++)
@ -1986,7 +1986,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1); write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1);
} }
} }
else if(angle>0 && angle<90) else if(angle > 0.0f && angle < 90.0f)
{ {
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y1;i<size;i++) for(int i=y1;i<size;i++)
@ -1999,7 +1999,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1); write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1);
} }
} }
else if(angle>90) else if(angle > 90.0f)
{ {
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y1;i++) for(int i=0;i<y1;i++)
@ -2017,7 +2017,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
{ {
// horizon line // horizon line
write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1); write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1);
if(angle==90) if(angle >= 90.0f)
{ {
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<size;i++) for(int i=0;i<size;i++)
@ -2143,7 +2143,7 @@ void calcHomeArrow(int16_t m_yaw)
d = 6371 * 1000 * c; d = 6371 * 1000 * c;
// Elevation v depends servo direction // Elevation v depends servo direction
if(d!=0) if(d > 0.0f)
elevation = 90-RAD2DEG(atanf(dAlt/d)); elevation = 90-RAD2DEG(atanf(dAlt/d));
else else
elevation = 0; elevation = 0;

4
flight/modules/Sensors/sensors.c
View File

@ -509,7 +509,9 @@ static void magOffsetEstimation(MagnetometerData *mag)
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]); delta[2] = -rate * (Rz - B_e[2]);
if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) { if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias.x += delta[0]; magBias.x += delta[0];
magBias.y += delta[1]; magBias.y += delta[1];
magBias.z += delta[2]; magBias.z += delta[2];

4
flight/modules/Stabilization/relay_tuning.c
View File

@ -60,8 +60,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
static uint32_t accumulated = 0; static uint32_t accumulated = 0;
const uint16_t DEGLITCH_TIME = 20; // ms const uint16_t DEGLITCH_TIME = 20; // ms
const float AMPLITUDE_ALPHA = 0.95; const float AMPLITUDE_ALPHA = 0.95f;
const float PERIOD_ALPHA = 0.95; const float PERIOD_ALPHA = 0.95f;
portTickType thisTime = xTaskGetTickCount(); portTickType thisTime = xTaskGetTickCount();

16
flight/modules/Stabilization/stabilization.c
View File

@ -248,7 +248,7 @@ static void stabilizationTask(void* parameters)
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
@ -264,7 +264,7 @@ static void stabilizationTask(void* parameters)
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
@ -288,7 +288,7 @@ static void stabilizationTask(void* parameters)
// Compute desired rate as input biased towards leveling // Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
} }
@ -310,7 +310,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
@ -320,7 +320,7 @@ static void stabilizationTask(void* parameters)
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
@ -334,12 +334,12 @@ static void stabilizationTask(void* parameters)
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f); actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f);
break; break;
default: default:
error = true; error = true;
@ -484,7 +484,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
// based on a time (in ms) rather than a fixed multiplier. The error between // based on a time (in ms) rather than a fixed multiplier. The error between
// 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.0025f;
if(settings.GyroTau < 0.0001f) 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

6
flight/modules/Stabilization/virtualflybar.c
View File

@ -31,6 +31,7 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <pios_math.h>
#include "stabilization.h" #include "stabilization.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
@ -92,9 +93,8 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
*/ */
int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
{ {
const float F_PI = (float) M_PI; float cy = cosf(DEG2RAD(z_gyro) * dT);
float cy = cosf(z_gyro / 180.0f * F_PI * dT); float sy = sinf(DEG2RAD(z_gyro) * dT);
float sy = sinf(z_gyro / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];

6
flight/modules/System/systemmod.c
View File

@ -461,9 +461,9 @@ static void updateStats()
#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.3f * PIOS_ADC_PinGet(0) / ((float)((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.43f; /* V */
const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */ const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25; stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
#endif #endif
SystemStatsSet(&stats); SystemStatsSet(&stats);
} }

4
flight/modules/Telemetry/telemetry.c
View File

@ -437,8 +437,8 @@ static void updateTelemetryStats()
// Update stats object // Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0); flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0); flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxFailures += utalkStats.rxErrors; flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors; flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries; flightStats.TxRetries += txRetries;

13
flight/modules/TxPID/txpid.c
View File

@ -178,7 +178,7 @@ static void updatePIDs(UAVObjEvent* ev)
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]); inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]); value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]);
} else { } else {
continue; continue;
} }
@ -302,7 +302,7 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM
// normalize input value to [0..1] // normalize input value to [0..1]
if (inMax <= inMin) if (inMax <= inMin)
val = 0.0; val = 0.0f;
else else
val = (val - inMin) / (inMax - inMin); val = (val - inMin) / (inMax - inMin);
@ -311,7 +311,7 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM
float t = outMin; float t = outMin;
outMin = outMax; outMin = outMax;
outMax = t; outMax = t;
val = 1.0 - val; val = 1.0f - val;
} }
return (outMax - outMin) * val + outMin; return (outMax - outMin) * val + outMin;
@ -323,7 +323,12 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM
*/ */
static uint8_t update(float *var, float val) static uint8_t update(float *var, float val)
{ {
if (*var != val) { /* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val; *var = val;
return 1; return 1;
} }

12
flight/pios/common/pios_rfm22b.c
View File

@ -1251,11 +1251,11 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
{ {
uint32_t datarate_bps = data_rate[datarate]; uint32_t datarate_bps = data_rate[datarate];
rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
// Generate a pseudo-random number from 0-8 to add to the delay // Generate a pseudo-random number from 0-8 to add to the delay
uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03;
rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5) * 4 + 4 + random; rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random;
// rfm22_if_filter_bandwidth // rfm22_if_filter_bandwidth
rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]); rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
@ -1330,11 +1330,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
} else { } else {
hbsel = 1; hbsel = 1;
} }
float freq_mhz = (float)(frequency_hz) / 1000000.0; float freq_mhz = (float)(frequency_hz) / 1000000.0f;
float xtal_freq_khz = 30000; float xtal_freq_khz = 30000.0f;
float sfreq = freq_mhz / (10.0 * (xtal_freq_khz / 30000.0) * (1 + hbsel)); float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel));
uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel);
uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0); uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f);
uint8_t fch = (fc >> 8) & 0xff; uint8_t fch = (fc >> 8) & 0xff;
uint8_t fcl = fc & 0xff; uint8_t fcl = fc & 0xff;

6
flight/pios/pios_sim_posix.h
View File

@ -58,6 +58,12 @@
#include <pios_sys.h> #include <pios_sys.h>
#include <pios_delay.h> #include <pios_delay.h>
#include <pios_led.h> #include <pios_led.h>
/* FIXME: simposix needs its own custom include directory into
* which a custom pios_led.h can be put that includes the following
* declaration. */
#ifdef PIOS_INCLUDE_LED
extern void PIOS_LED_Init(void);
#endif
#include <pios_irq.h> #include <pios_irq.h>
#include <pios_sdcard.h> #include <pios_sdcard.h>
#include <pios_udp.h> #include <pios_udp.h>

2
flight/pios/posix/pios_sys.c
View File

@ -28,7 +28,7 @@
/* Project Includes */ /* Project Includes */
#include "pios.h" #include <pios.h>
#if defined(PIOS_INCLUDE_SYS) #if defined(PIOS_INCLUDE_SYS)

2
flight/pios/stm32f10x/pios_rtc.c
View File

@ -80,7 +80,7 @@ uint32_t PIOS_RTC_Counter()
*/ */
float PIOS_RTC_Rate() float PIOS_RTC_Rate()
{ {
return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALER); return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER);
} }
float PIOS_RTC_MsPerTick() float PIOS_RTC_MsPerTick()

4
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c
View File

@ -1986,7 +1986,7 @@ void USB_OTG_ActiveRemoteWakeup(USB_OTG_CORE_HANDLE *pdev)
if(pdev->cfg.low_power) if(pdev->cfg.low_power)
{ {
/* un-gate USB Core clock */ /* un-gate USB Core clock */
power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL); power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0; power.b.gatehclk = 0;
power.b.stoppclk = 0; power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32); USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
@ -2020,7 +2020,7 @@ void USB_OTG_UngateClock(USB_OTG_CORE_HANDLE *pdev)
if(dsts.b.suspsts == 1) if(dsts.b.suspsts == 1)
{ {
/* un-gate USB Core clock */ /* un-gate USB Core clock */
power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL); power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0; power.b.gatehclk = 0;
power.b.stoppclk = 0; power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32); USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);

2
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd.c
View File

@ -264,7 +264,7 @@ uint32_t DCD_EP_Tx ( USB_OTG_CORE_HANDLE *pdev,
/* Setup and start the Transfer */ /* Setup and start the Transfer */
ep->is_in = 1; ep->is_in = 1;
ep->num = ep_addr & 0x7F; ep->num = ep_addr & 0x7F;
ep->xfer_buff = pbuf; ep->xfer_buff = (uint8_t*)pbuf;
ep->dma_addr = (uint32_t)pbuf; ep->dma_addr = (uint32_t)pbuf;
ep->xfer_count = 0; ep->xfer_count = 0;
ep->xfer_len = buf_len; ep->xfer_len = buf_len;

2
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
View File

@ -358,7 +358,7 @@ static uint32_t DCD_HandleResume_ISR(USB_OTG_CORE_HANDLE *pdev)
if(pdev->cfg.low_power) if(pdev->cfg.low_power)
{ {
/* un-gate USB Core clock */ /* un-gate USB Core clock */
power.d32 = USB_OTG_READ_REG32(&pdev->regs.PCGCCTL); power.d32 = USB_OTG_READ_REG32(pdev->regs.PCGCCTL);
power.b.gatehclk = 0; power.b.gatehclk = 0;
power.b.stoppclk = 0; power.b.stoppclk = 0;
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32); USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);

2
flight/pios/stm32f4xx/pios_i2c.c
View File

@ -668,7 +668,7 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
* failures at this transition which previously resulted * failures at this transition which previously resulted
* in spinning on this bit in the ISR forever. * in spinning on this bit in the ISR forever.
*/ */
for (guard = 1e6; /* FIXME: should use the configured bus timeout */ for (guard = 1000000; /* FIXME: should use the configured bus timeout */
guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--) guard && (i2c_adapter->cfg->regs->CR1 & I2C_CR1_STOP); guard--)
continue; continue;
if (!guard) { if (!guard) {

2
flight/pios/stm32f4xx/pios_rtc.c
View File

@ -88,7 +88,7 @@ uint32_t PIOS_RTC_Counter()
*/ */
float PIOS_RTC_Rate() float PIOS_RTC_Rate()
{ {
return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALER); return (float) (8e6f / 128.0f) / (1 + PIOS_RTC_PRESCALER);
} }
float PIOS_RTC_MsPerTick() float PIOS_RTC_MsPerTick()

2
flight/targets/boards/coptercontrol/pios_board.h
View File

@ -210,7 +210,7 @@ extern uint32_t pios_com_debug_id;
// Currently analog acquistion hard coded at 480 Hz // Currently analog acquistion hard coded at 480 Hz
// PCKL2 = HCLK / 16 // PCKL2 = HCLK / 16
// ADCCLK = PCLK2 / 2 // ADCCLK = PCLK2 / 2
#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) #define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
#define PIOS_ADC_MAX_OVERSAMPLING 36 #define PIOS_ADC_MAX_OVERSAMPLING 36
//------------------------ //------------------------

14
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -209,9 +209,9 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info)
} }
else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 )
{ {
initfields.append( QString("\tdata.%1 = %2;\r\n") initfields.append( QString("\tdata.%1 = %2f;\r\n")
.arg( info->fields[n]->name ) .arg(info->fields[n]->name)
.arg( info->fields[n]->defaultValues[0].toFloat() ) ); .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6));
} }
else else
{ {
@ -234,10 +234,10 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info)
} }
else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 )
{ {
initfields.append( QString("\tdata.%1[%2] = %3;\r\n") initfields.append( QString("\tdata.%1[%2] = %3f;\r\n")
.arg( info->fields[n]->name ) .arg(info->fields[n]->name)
.arg( idx ) .arg(idx)
.arg( info->fields[n]->defaultValues[idx].toFloat() ) ); .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6));
} }
else else
{ {

7
make/common-defs.mk
View File

@ -119,14 +119,11 @@ CFLAGS += -g$(DEBUGF)
CFLAGS += -mapcs-frame CFLAGS += -mapcs-frame
CFLAGS += -fomit-frame-pointer CFLAGS += -fomit-frame-pointer
CFLAGS += -Wall CFLAGS += -Wall
CFLAGS += -Wfloat-equal -Wunsuffixed-float-constants
CFLAGS += -Werror
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
# FIXME: stm32f4xx library raises strict aliasing and const qualifier warnings
ifneq ($(MCU),cortex-m4)
CFLAGS += -Werror
endif
ifeq ($(DEBUG), YES) ifeq ($(DEBUG), YES)
CFLAGS += -DDEBUG CFLAGS += -DDEBUG
else else