mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-931: Makes flight code compile with -Wfloat-equal and -Wunsuffixed-float-constants enabled.
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)
parent
80c917591e
commit
7937ae6296
@ -37,8 +37,8 @@
|
||||
// ****** convert Lat,Lon,Alt to ECEF ************
|
||||
void LLA2ECEF(float LLA[3], float ECEF[3])
|
||||
{
|
||||
const float a = 6378137.0; // Equatorial Radius
|
||||
const float e = 8.1819190842622e-2; // Eccentricity
|
||||
const float a = 6378137.0f; // Equatorial Radius
|
||||
const float e = 8.1819190842622e-2f; // Eccentricity
|
||||
float sinLat, sinLon, cosLat, cosLon;
|
||||
float N;
|
||||
|
||||
@ -66,7 +66,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
|
||||
* 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
|
||||
float x = ECEF[0], y = ECEF[1], z = ECEF[2];
|
||||
float Lat, N, NplusH, delta, esLat;
|
||||
|
||||
@ -59,97 +59,97 @@
|
||||
|
||||
// http://reviews.openpilot.org/cru/OPReview-436#c6476 :
|
||||
// first column not used but it will be optimized out by compiler
|
||||
static const float CoeffFile[91][6] = { {0, 0, 0, 0, 0, 0},
|
||||
{1, 0, -29496.6, 0.0, 11.6, 0.0},
|
||||
{1, 1, -1586.3, 4944.4, 16.5, -25.9},
|
||||
{2, 0, -2396.6, 0.0, -12.1, 0.0},
|
||||
{2, 1, 3026.1, -2707.7, -4.4, -22.5},
|
||||
{2, 2, 1668.6, -576.1, 1.9, -11.8},
|
||||
{3, 0, 1340.1, 0.0, 0.4, 0.0},
|
||||
{3, 1, -2326.2, -160.2, -4.1, 7.3},
|
||||
{3, 2, 1231.9, 251.9, -2.9, -3.9},
|
||||
{3, 3, 634.0, -536.6, -7.7, -2.6},
|
||||
{4, 0, 912.6, 0.0, -1.8, 0.0},
|
||||
{4, 1, 808.9, 286.4, 2.3, 1.1},
|
||||
{4, 2, 166.7, -211.2, -8.7, 2.7},
|
||||
{4, 3, -357.1, 164.3, 4.6, 3.9},
|
||||
{4, 4, 89.4, -309.1, -2.1, -0.8},
|
||||
{5, 0, -230.9, 0.0, -1.0, 0.0},
|
||||
{5, 1, 357.2, 44.6, 0.6, 0.4},
|
||||
{5, 2, 200.3, 188.9, -1.8, 1.8},
|
||||
{5, 3, -141.1, -118.2, -1.0, 1.2},
|
||||
{5, 4, -163.0, 0.0, 0.9, 4.0},
|
||||
{5, 5, -7.8, 100.9, 1.0, -0.6},
|
||||
{6, 0, 72.8, 0.0, -0.2, 0.0},
|
||||
{6, 1, 68.6, -20.8, -0.2, -0.2},
|
||||
{6, 2, 76.0, 44.1, -0.1, -2.1},
|
||||
{6, 3, -141.4, 61.5, 2.0, -0.4},
|
||||
{6, 4, -22.8, -66.3, -1.7, -0.6},
|
||||
{6, 5, 13.2, 3.1, -0.3, 0.5},
|
||||
{6, 6, -77.9, 55.0, 1.7, 0.9},
|
||||
{7, 0, 80.5, 0.0, 0.1, 0.0},
|
||||
{7, 1, -75.1, -57.9, -0.1, 0.7},
|
||||
{7, 2, -4.7, -21.1, -0.6, 0.3},
|
||||
{7, 3, 45.3, 6.5, 1.3, -0.1},
|
||||
{7, 4, 13.9, 24.9, 0.4, -0.1},
|
||||
{7, 5, 10.4, 7.0, 0.3, -0.8},
|
||||
{7, 6, 1.7, -27.7, -0.7, -0.3},
|
||||
{7, 7, 4.9, -3.3, 0.6, 0.3},
|
||||
{8, 0, 24.4, 0.0, -0.1, 0.0},
|
||||
{8, 1, 8.1, 11.0, 0.1, -0.1},
|
||||
{8, 2, -14.5, -20.0, -0.6, 0.2},
|
||||
{8, 3, -5.6, 11.9, 0.2, 0.4},
|
||||
{8, 4, -19.3, -17.4, -0.2, 0.4},
|
||||
{8, 5, 11.5, 16.7, 0.3, 0.1},
|
||||
{8, 6, 10.9, 7.0, 0.3, -0.1},
|
||||
{8, 7, -14.1, -10.8, -0.6, 0.4},
|
||||
{8, 8, -3.7, 1.7, 0.2, 0.3},
|
||||
{9, 0, 5.4, 0.0, 0.0, 0.0},
|
||||
{9, 1, 9.4, -20.5, -0.1, 0.0},
|
||||
{9, 2, 3.4, 11.5, 0.0, -0.2},
|
||||
{9, 3, -5.2, 12.8, 0.3, 0.0},
|
||||
{9, 4, 3.1, -7.2, -0.4, -0.1},
|
||||
{9, 5, -12.4, -7.4, -0.3, 0.1},
|
||||
{9, 6, -0.7, 8.0, 0.1, 0.0},
|
||||
{9, 7, 8.4, 2.1, -0.1, -0.2},
|
||||
{9, 8, -8.5, -6.1, -0.4, 0.3},
|
||||
{9, 9, -10.1, 7.0, -0.2, 0.2},
|
||||
{10, 0, -2.0, 0.0, 0.0, 0.0},
|
||||
{10, 1, -6.3, 2.8, 0.0, 0.1},
|
||||
{10, 2, 0.9, -0.1, -0.1, -0.1},
|
||||
{10, 3, -1.1, 4.7, 0.2, 0.0},
|
||||
{10, 4, -0.2, 4.4, 0.0, -0.1},
|
||||
{10, 5, 2.5, -7.2, -0.1, -0.1},
|
||||
{10, 6, -0.3, -1.0, -0.2, 0.0},
|
||||
{10, 7, 2.2, -3.9, 0.0, -0.1},
|
||||
{10, 8, 3.1, -2.0, -0.1, -0.2},
|
||||
{10, 9, -1.0, -2.0, -0.2, 0.0},
|
||||
{10, 10, -2.8, -8.3, -0.2, -0.1},
|
||||
{11, 0, 3.0, 0.0, 0.0, 0.0},
|
||||
{11, 1, -1.5, 0.2, 0.0, 0.0},
|
||||
{11, 2, -2.1, 1.7, 0.0, 0.1},
|
||||
{11, 3, 1.7, -0.6, 0.1, 0.0},
|
||||
{11, 4, -0.5, -1.8, 0.0, 0.1},
|
||||
{11, 5, 0.5, 0.9, 0.0, 0.0},
|
||||
{11, 6, -0.8, -0.4, 0.0, 0.1},
|
||||
{11, 7, 0.4, -2.5, 0.0, 0.0},
|
||||
{11, 8, 1.8, -1.3, 0.0, -0.1},
|
||||
{11, 9, 0.1, -2.1, 0.0, -0.1},
|
||||
{11, 10, 0.7, -1.9, -0.1, 0.0},
|
||||
{11, 11, 3.8, -1.8, 0.0, -0.1},
|
||||
{12, 0, -2.2, 0.0, 0.0, 0.0},
|
||||
{12, 1, -0.2, -0.9, 0.0, 0.0},
|
||||
{12, 2, 0.3, 0.3, 0.1, 0.0},
|
||||
{12, 3, 1.0, 2.1, 0.1, 0.0},
|
||||
{12, 4, -0.6, -2.5, -0.1, 0.0},
|
||||
{12, 5, 0.9, 0.5, 0.0, 0.0},
|
||||
{12, 6, -0.1, 0.6, 0.0, 0.1},
|
||||
{12, 7, 0.5, 0.0, 0.0, 0.0},
|
||||
{12, 8, -0.4, 0.1, 0.0, 0.0},
|
||||
{12, 9, -0.4, 0.3, 0.0, 0.0},
|
||||
{12, 10, 0.2, -0.9, 0.0, 0.0},
|
||||
{12, 11, -0.8, -0.2, -0.1, 0.0},
|
||||
{12, 12, 0.0, 0.9, 0.1, 0.0}
|
||||
static const float CoeffFile[91][6] = { {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{1.0f, 0.0f, -29496.6f, 0.0f, 11.6f, 0.0f},
|
||||
{1.0f, 1.0f, -1586.3f, 4944.4f, 16.5f, -25.9f},
|
||||
{2.0f, 0.0f, -2396.6f, 0.0f, -12.1f, 0.0f},
|
||||
{2.0f, 1.0f, 3026.1f, -2707.7f, -4.4f, -22.5f},
|
||||
{2.0f, 2.0f, 1668.6f, -576.1f, 1.9f, -11.8f},
|
||||
{3.0f, 0.0f, 1340.1f, 0.0f, 0.4f, 0.0f},
|
||||
{3.0f, 1.0f, -2326.2f, -160.2f, -4.1f, 7.3f},
|
||||
{3.0f, 2.0f, 1231.9f, 251.9f, -2.9f, -3.9f},
|
||||
{3.0f, 3.0f, 634.0f, -536.6f, -7.7f, -2.6f},
|
||||
{4.0f, 0.0f, 912.6f, 0.0f, -1.8f, 0.0f},
|
||||
{4.0f, 1.0f, 808.9f, 286.4f, 2.3f, 1.1f},
|
||||
{4.0f, 2.0f, 166.7f, -211.2f, -8.7f, 2.7f},
|
||||
{4.0f, 3.0f, -357.1f, 164.3f, 4.6f, 3.9f},
|
||||
{4.0f, 4.0f, 89.4f, -309.1f, -2.1f, -0.8f},
|
||||
{5.0f, 0.0f, -230.9f, 0.0f, -1.0f, 0.0f},
|
||||
{5.0f, 1.0f, 357.2f, 44.6f, 0.6f, 0.4f},
|
||||
{5.0f, 2.0f, 200.3f, 188.9f, -1.8f, 1.8f},
|
||||
{5.0f, 3.0f, -141.1f, -118.2f, -1.0f, 1.2f},
|
||||
{5.0f, 4.0f, -163.0f, 0.0f, 0.9f, 4.0f},
|
||||
{5.0f, 5.0f, -7.8f, 100.9f, 1.0f, -0.6f},
|
||||
{6.0f, 0.0f, 72.8f, 0.0f, -0.2f, 0.0f},
|
||||
{6.0f, 1.0f, 68.6f, -20.8f, -0.2f, -0.2f},
|
||||
{6.0f, 2.0f, 76.0f, 44.1f, -0.1f, -2.1f},
|
||||
{6.0f, 3.0f, -141.4f, 61.5f, 2.0f, -0.4f},
|
||||
{6.0f, 4.0f, -22.8f, -66.3f, -1.7f, -0.6f},
|
||||
{6.0f, 5.0f, 13.2f, 3.1f, -0.3f, 0.5f},
|
||||
{6.0f, 6.0f, -77.9f, 55.0f, 1.7f, 0.9f},
|
||||
{7.0f, 0.0f, 80.5f, 0.0f, 0.1f, 0.0f},
|
||||
{7.0f, 1.0f, -75.1f, -57.9f, -0.1f, 0.7f},
|
||||
{7.0f, 2.0f, -4.7f, -21.1f, -0.6f, 0.3f},
|
||||
{7.0f, 3.0f, 45.3f, 6.5f, 1.3f, -0.1f},
|
||||
{7.0f, 4.0f, 13.9f, 24.9f, 0.4f, -0.1f},
|
||||
{7.0f, 5.0f, 10.4f, 7.0f, 0.3f, -0.8f},
|
||||
{7.0f, 6.0f, 1.7f, -27.7f, -0.7f, -0.3f},
|
||||
{7.0f, 7.0f, 4.9f, -3.3f, 0.6f, 0.3f},
|
||||
{8.0f, 0.0f, 24.4f, 0.0f, -0.1f, 0.0f},
|
||||
{8.0f, 1.0f, 8.1f, 11.0f, 0.1f, -0.1f},
|
||||
{8.0f, 2.0f, -14.5f, -20.0f, -0.6f, 0.2f},
|
||||
{8.0f, 3.0f, -5.6f, 11.9f, 0.2f, 0.4f},
|
||||
{8.0f, 4.0f, -19.3f, -17.4f, -0.2f, 0.4f},
|
||||
{8.0f, 5.0f, 11.5f, 16.7f, 0.3f, 0.1f},
|
||||
{8.0f, 6.0f, 10.9f, 7.0f, 0.3f, -0.1f},
|
||||
{8.0f, 7.0f, -14.1f, -10.8f, -0.6f, 0.4f},
|
||||
{8.0f, 8.0f, -3.7f, 1.7f, 0.2f, 0.3f},
|
||||
{9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f},
|
||||
{9.0f, 1.0f, 9.4f, -20.5f, -0.1f, 0.0f},
|
||||
{9.0f, 2.0f, 3.4f, 11.5f, 0.0f, -0.2f},
|
||||
{9.0f, 3.0f, -5.2f, 12.8f, 0.3f, 0.0f},
|
||||
{9.0f, 4.0f, 3.1f, -7.2f, -0.4f, -0.1f},
|
||||
{9.0f, 5.0f, -12.4f, -7.4f, -0.3f, 0.1f},
|
||||
{9.0f, 6.0f, -0.7f, 8.0f, 0.1f, 0.0f},
|
||||
{9.0f, 7.0f, 8.4f, 2.1f, -0.1f, -0.2f},
|
||||
{9.0f, 8.0f, -8.5f, -6.1f, -0.4f, 0.3f},
|
||||
{9.0f, 9.0f, -10.1f, 7.0f, -0.2f, 0.2f},
|
||||
{10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f},
|
||||
{10.0f, 1.0f, -6.3f, 2.8f, 0.0f, 0.1f},
|
||||
{10.0f, 2.0f, 0.9f, -0.1f, -0.1f, -0.1f},
|
||||
{10.0f, 3.0f, -1.1f, 4.7f, 0.2f, 0.0f},
|
||||
{10.0f, 4.0f, -0.2f, 4.4f, 0.0f, -0.1f},
|
||||
{10.0f, 5.0f, 2.5f, -7.2f, -0.1f, -0.1f},
|
||||
{10.0f, 6.0f, -0.3f, -1.0f, -0.2f, 0.0f},
|
||||
{10.0f, 7.0f, 2.2f, -3.9f, 0.0f, -0.1f},
|
||||
{10.0f, 8.0f, 3.1f, -2.0f, -0.1f, -0.2f},
|
||||
{10.0f, 9.0f, -1.0f, -2.0f, -0.2f, 0.0f},
|
||||
{10.0f, 10.0f, -2.8f, -8.3f, -0.2f, -0.1f},
|
||||
{11.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f},
|
||||
{11.0f, 1.0f, -1.5f, 0.2f, 0.0f, 0.0f},
|
||||
{11.0f, 2.0f, -2.1f, 1.7f, 0.0f, 0.1f},
|
||||
{11.0f, 3.0f, 1.7f, -0.6f, 0.1f, 0.0f},
|
||||
{11.0f, 4.0f, -0.5f, -1.8f, 0.0f, 0.1f},
|
||||
{11.0f, 5.0f, 0.5f, 0.9f, 0.0f, 0.0f},
|
||||
{11.0f, 6.0f, -0.8f, -0.4f, 0.0f, 0.1f},
|
||||
{11.0f, 7.0f, 0.4f, -2.5f, 0.0f, 0.0f},
|
||||
{11.0f, 8.0f, 1.8f, -1.3f, 0.0f, -0.1f},
|
||||
{11.0f, 9.0f, 0.1f, -2.1f, 0.0f, -0.1f},
|
||||
{11.0f, 10.0f, 0.7f, -1.9f, -0.1f, 0.0f},
|
||||
{11.0f, 11.0f, 3.8f, -1.8f, 0.0f, -0.1f},
|
||||
{12.0f, 0.0f, -2.2f, 0.0f, 0.0f, 0.0f},
|
||||
{12.0f, 1.0f, -0.2f, -0.9f, 0.0f, 0.0f},
|
||||
{12.0f, 2.0f, 0.3f, 0.3f, 0.1f, 0.0f},
|
||||
{12.0f, 3.0f, 1.0f, 2.1f, 0.1f, 0.0f},
|
||||
{12.0f, 4.0f, -0.6f, -2.5f, -0.1f, 0.0f},
|
||||
{12.0f, 5.0f, 0.9f, 0.5f, 0.0f, 0.0f},
|
||||
{12.0f, 6.0f, -0.1f, 0.6f, 0.0f, 0.1f},
|
||||
{12.0f, 7.0f, 0.5f, 0.0f, 0.0f, 0.0f},
|
||||
{12.0f, 8.0f, -0.4f, 0.1f, 0.0f, 0.0f},
|
||||
{12.0f, 9.0f, -0.4f, 0.3f, 0.0f, 0.0f},
|
||||
{12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f},
|
||||
{12.0f, 11.0f, -0.8f, -0.2f, -0.1f, 0.0f},
|
||||
{12.0f, 12.0f, 0.0f, 0.9f, 0.1f, 0.0f}
|
||||
};
|
||||
|
||||
static WMMtype_Ellipsoid *Ellip = NULL;
|
||||
@ -175,12 +175,12 @@ int WMM_Initialize()
|
||||
if (!MagneticModel) return -2; // invalid pointer
|
||||
|
||||
// Sets WGS-84 parameters
|
||||
Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km
|
||||
Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km
|
||||
Ellip->fla = 1 / 298.257223563; // flattening
|
||||
Ellip->a = 6378.137f; // semi-major axis of the ellipsoid in km
|
||||
Ellip->b = 6356.7523142f; // semi-minor axis of the ellipsoid in km
|
||||
Ellip->fla = 1.0f / 298.257223563f; // flattening
|
||||
Ellip->eps = sqrt(1 - (Ellip->b * Ellip->b) / (Ellip->a * Ellip->a)); // first eccentricity
|
||||
Ellip->epssq = (Ellip->eps * Ellip->eps); // first eccentricity squared
|
||||
Ellip->re = 6371.2; // Earth's radius in km
|
||||
Ellip->re = 6371.2f; // Earth's radius in km
|
||||
|
||||
// Sets Magnetic Model parameters
|
||||
MagneticModel->nMax = WMM_MAX_MODEL_DEGREES;
|
||||
@ -188,15 +188,15 @@ int WMM_Initialize()
|
||||
MagneticModel->SecularVariationUsed = 0;
|
||||
|
||||
// Really, Really needs to be read from a file - out of date in 2015 at latest
|
||||
MagneticModel->EditionDate = 5.7863328170559505e-307;
|
||||
MagneticModel->epoch = 2010.0;
|
||||
MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */
|
||||
MagneticModel->epoch = 2010.0f;
|
||||
sprintf(MagneticModel->ModelName, "WMM-2010");
|
||||
|
||||
return 0; // OK
|
||||
}
|
||||
|
||||
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 error
|
||||
|
||||
@ -245,7 +245,7 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
|
||||
CoordGeodetic->phi = Lat;
|
||||
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)
|
||||
returned = -7; // error
|
||||
}
|
||||
@ -525,9 +525,9 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
uint16_t m, n, index;
|
||||
float cos_phi;
|
||||
|
||||
MagneticResults->Bz = 0.0;
|
||||
MagneticResults->By = 0.0;
|
||||
MagneticResults->Bx = 0.0;
|
||||
MagneticResults->Bz = 0.0f;
|
||||
MagneticResults->By = 0.0f;
|
||||
MagneticResults->Bx = 0.0f;
|
||||
|
||||
for (n = 1; n <= MagneticModel->nMax; n++)
|
||||
{
|
||||
@ -607,9 +607,9 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
|
||||
MagneticModel->SecularVariationUsed = TRUE;
|
||||
|
||||
MagneticResults->Bz = 0.0;
|
||||
MagneticResults->By = 0.0;
|
||||
MagneticResults->Bx = 0.0;
|
||||
MagneticResults->Bz = 0.0f;
|
||||
MagneticResults->By = 0.0f;
|
||||
MagneticResults->Bx = 0.0f;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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(f2);
|
||||
@ -840,7 +844,9 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
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)
|
||||
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 = sqrtf((1.0f - x) * (1.0f + x));
|
||||
pm2 = 1.0;
|
||||
pm2 = 1.0f;
|
||||
Pcup[0] = 1.0f;
|
||||
dPcup[0] = 0.0;
|
||||
dPcup[0] = 0.0f;
|
||||
if (nMax == 0)
|
||||
{
|
||||
FREE(PreSqr);
|
||||
@ -889,7 +895,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
}
|
||||
|
||||
pmm = PreSqr[2] * scalef;
|
||||
rescalem = 1.0 / scalef;
|
||||
rescalem = 1.0f / scalef;
|
||||
kstart = 0;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
Pcup[0] = 1.0;
|
||||
dPcup[0] = 0.0;
|
||||
Pcup[0] = 1.0f;
|
||||
dPcup[0] = 0.0f;
|
||||
|
||||
/*sinf (geocentric latitude) - sin_phi */
|
||||
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
|
||||
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++)
|
||||
{
|
||||
index = (n * (n + 1) / 2);
|
||||
@ -1084,9 +1090,9 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
return -1; // memory allocation error
|
||||
|
||||
PcupS[0] = 1;
|
||||
schmidtQuasiNorm1 = 1.0;
|
||||
schmidtQuasiNorm1 = 1.0f;
|
||||
|
||||
MagneticResults->By = 0.0;
|
||||
MagneticResults->By = 0.0f;
|
||||
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
|
||||
|
||||
for (n = 1; n <= MagneticModel->nMax; n++)
|
||||
@ -1150,9 +1156,9 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
return -1; // memory allocation error
|
||||
|
||||
PcupS[0] = 1;
|
||||
schmidtQuasiNorm1 = 1.0;
|
||||
schmidtQuasiNorm1 = 1.0f;
|
||||
|
||||
MagneticResults->By = 0.0;
|
||||
MagneticResults->By = 0.0f;
|
||||
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
|
||||
|
||||
for (n = 1; n <= MagneticModel->nMaxSecVar; n++)
|
||||
|
||||
@ -39,7 +39,7 @@ static float bound(float val, float range);
|
||||
static float deriv_tau = 7.9577e-3f;
|
||||
|
||||
//! 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
|
||||
@ -58,7 +58,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
|
||||
float diff = (err - pid->lastErr);
|
||||
float dterm = 0;
|
||||
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);
|
||||
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 diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
|
||||
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);
|
||||
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
|
||||
|
||||
@ -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
|
||||
* 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
|
||||
* capable of doing this.
|
||||
*/
|
||||
@ -287,14 +287,14 @@ static uint my_strlen(char *str)
|
||||
// It is more useful when implementing a walking-string function.
|
||||
//****************************************************************************
|
||||
static const double round_nums[8] = {
|
||||
0.5,
|
||||
0.05,
|
||||
0.005,
|
||||
0.0005,
|
||||
0.00005,
|
||||
0.000005,
|
||||
0.0000005,
|
||||
0.00000005
|
||||
0.5L,
|
||||
0.05L,
|
||||
0.005L,
|
||||
0.0005L,
|
||||
0.00005L,
|
||||
0.000005L,
|
||||
0.0000005L,
|
||||
0.00000005L
|
||||
} ;
|
||||
|
||||
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
|
||||
//*******************************************
|
||||
if (dbl < 0.0) {
|
||||
if (dbl < 0.0L) {
|
||||
*output++ = '-' ;
|
||||
dbl *= -1.0 ;
|
||||
dbl *= -1.0L ;
|
||||
} else {
|
||||
if (use_leading_plus) {
|
||||
*output++ = '+' ;
|
||||
|
||||
@ -431,7 +431,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
|
||||
lastResult[index] = result;
|
||||
result += accumulator;
|
||||
if(period !=0)
|
||||
if(period > 0.0f)
|
||||
{
|
||||
if(accumulator > 0.0f)
|
||||
{
|
||||
|
||||
@ -177,10 +177,13 @@ static void altitudeHoldTask(void *parameters)
|
||||
static float z[4] = {0, 0, 0, 0};
|
||||
float z_new[4];
|
||||
float P[4][4], K[4][2], x[2];
|
||||
float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7};
|
||||
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}};
|
||||
float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f};
|
||||
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 float accels_accum[3] = {0,0,0};
|
||||
static float accels_accum[3] = {0.0f, 0.0f, 0.0f};
|
||||
float dT;
|
||||
static float S[2] = {1.0f,10.0f};
|
||||
|
||||
|
||||
@ -92,7 +92,7 @@ static bool accel_filter_enabled = false;
|
||||
static float accels_filtered[3];
|
||||
static float grot_filtered[3];
|
||||
static float yawBiasRate = 0;
|
||||
static float gyroGain = 0.42;
|
||||
static float gyroGain = 0.42f;
|
||||
static int16_t accelbias[3];
|
||||
static float q[4] = {1,0,0,0};
|
||||
static float R[3][3];
|
||||
@ -217,16 +217,16 @@ static void AttitudeTask(void *parameters)
|
||||
|
||||
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||
// For first 7 seconds use accels to get gyro bias
|
||||
accelKp = 1;
|
||||
accelKi = 0.9;
|
||||
yawBiasRate = 0.01;
|
||||
accelKp = 1.0f;
|
||||
accelKi = 0.9f;
|
||||
yawBiasRate = 0.01f;
|
||||
accel_filter_enabled = false;
|
||||
init = 0;
|
||||
}
|
||||
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
|
||||
accelKp = 1;
|
||||
accelKi = 0.9;
|
||||
yawBiasRate = 0.01;
|
||||
accelKp = 1.0f;
|
||||
accelKi = 0.9f;
|
||||
yawBiasRate = 0.01f;
|
||||
accel_filter_enabled = false;
|
||||
init = 0;
|
||||
} else if (init == 0) {
|
||||
@ -457,7 +457,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
portTickType thisSysTime = xTaskGetTickCount();
|
||||
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;
|
||||
|
||||
// Bad practice to assume structure order, but saves memory
|
||||
@ -535,18 +535,19 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
|
||||
// Renomalize
|
||||
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.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if((fabsf(qmag) < 1e-3f) || (qmag != qmag)) {
|
||||
if((fabsf(qmag) < 1e-3f) || isnan(qmag)) {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 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;
|
||||
@ -571,7 +572,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
|
||||
gyroGain = attitudeSettings.GyroGain;
|
||||
|
||||
// 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) {
|
||||
accel_alpha = 0; // not trusting this to resolve to 0
|
||||
accel_filter_enabled = false;
|
||||
|
||||
@ -315,14 +315,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
|
||||
|
||||
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||
// For first 7 seconds use accels to get gyro bias
|
||||
attitudeSettings.AccelKp = 1;
|
||||
attitudeSettings.AccelKi = 0.9;
|
||||
attitudeSettings.YawBiasRate = 0.23;
|
||||
attitudeSettings.AccelKp = 1.0f;
|
||||
attitudeSettings.AccelKi = 0.9f;
|
||||
attitudeSettings.YawBiasRate = 0.23f;
|
||||
magKp = 1;
|
||||
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
|
||||
attitudeSettings.AccelKp = 1;
|
||||
attitudeSettings.AccelKi = 0.9;
|
||||
attitudeSettings.YawBiasRate = 0.23;
|
||||
attitudeSettings.AccelKp = 1.0f;
|
||||
attitudeSettings.AccelKi = 0.9f;
|
||||
attitudeSettings.YawBiasRate = 0.23f;
|
||||
magKp = 1;
|
||||
init = 0;
|
||||
} else if (init == 0) {
|
||||
@ -373,7 +373,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
|
||||
MagnetometerGet(&mag);
|
||||
|
||||
// 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);
|
||||
|
||||
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.
|
||||
// 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[1] = 0;
|
||||
q[2] = 0;
|
||||
@ -601,10 +601,10 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
|
||||
// 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
|
||||
// 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
|
||||
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
|
||||
|
||||
@ -176,7 +176,7 @@ static void magbaroTask(void *parameters)
|
||||
alt_ds_count = 0;
|
||||
|
||||
// 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;
|
||||
|
||||
// Convert from Pa to kPa
|
||||
@ -184,7 +184,7 @@ static void magbaroTask(void *parameters)
|
||||
alt_ds_pres = 0;
|
||||
|
||||
// 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
|
||||
BaroAltitudeSet(&data);
|
||||
|
||||
@ -508,25 +508,19 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
|
||||
if (
|
||||
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
|
||||
&& velocityActual.Down > 0 // we ARE going down
|
||||
&& descentspeedDesired < 0 // we WANT to go up
|
||||
&& airspeedError > 0 // we are too slow already
|
||||
)
|
||||
{
|
||||
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
|
||||
velocityActual.Down > 0 && // we ARE going down
|
||||
descentspeedDesired < 0 && // we WANT to go up
|
||||
airspeedError > 0) { // we are too slow already
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
|
||||
if (
|
||||
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
|
||||
&& velocityActual.Down < 0 // we ARE going up
|
||||
&& descentspeedDesired > 0 // we WANT to go down
|
||||
&& airspeedError < 0 // we are too fast already
|
||||
)
|
||||
{
|
||||
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum
|
||||
velocityActual.Down < 0 && // we ARE going up
|
||||
descentspeedDesired > 0 && // we WANT to go down
|
||||
airspeedError < 0 ) { // we are too fast already
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
|
||||
result = 0;
|
||||
}
|
||||
@ -558,24 +552,20 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
|
||||
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
|
||||
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
|
||||
pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
|
||||
|
||||
// Error condition: high speed dive
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
|
||||
if (
|
||||
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
|
||||
&& velocityActual.Down > 0 // we ARE going down
|
||||
&& descentspeedDesired < 0 // we WANT to go up
|
||||
&& airspeedError < 0 // we are too fast already
|
||||
) {
|
||||
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
|
||||
velocityActual.Down > 0 && // we ARE going down
|
||||
descentspeedDesired < 0 && // we WANT to go up
|
||||
airspeedError < 0 ) { // we are too fast already
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired roll command
|
||||
*/
|
||||
|
||||
@ -90,7 +90,9 @@ static char* gps_rx_buffer;
|
||||
static uint32_t timeOfLastCommandMs;
|
||||
static uint32_t timeOfLastUpdateMs;
|
||||
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
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
|
||||
//criteria for GPS-OK taken from this post...
|
||||
//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)) {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
@ -275,14 +277,14 @@ static void gpsTask(void *parameters)
|
||||
*/
|
||||
static float GravityAccel(float latitude, float longitude, float altitude)
|
||||
{
|
||||
// WGS84 gravity model. The effect of gravity over latitude is strong
|
||||
// enough to change the estimated accelerometer bias in those apps.
|
||||
double sinsq = sin((double)latitude);
|
||||
/* WGS84 gravity model. The effect of gravity over latitude is strong
|
||||
* enough to change the estimated accelerometer bias in those apps. */
|
||||
float sinsq = sinf(latitude);
|
||||
sinsq *= sinsq;
|
||||
// Likewise, over the altitude range of a high-altitude balloon, the effect
|
||||
// due to change in altitude can also affect the model.
|
||||
return (float)(9.7803267714 * (1 + 0.00193185138639*sinsq) / sqrt(1 - 0.00669437999013*sinsq)
|
||||
- 3.086e-6*altitude);
|
||||
/* Likewise, over the altitude range of a high-altitude balloon, the effect
|
||||
* due to change in altitude can also affect the model. */
|
||||
return (9.7803267714f * (1.0f + 0.00193185138639f*sinsq) / sqrtf(1.0f - 0.00669437999013f*sinsq)
|
||||
- 3.086e-6f*altitude);
|
||||
}
|
||||
|
||||
// ****************
|
||||
@ -296,21 +298,23 @@ static void setHomeLocation(GPSPositionData * gpsData)
|
||||
|
||||
if (gps.Year >= 2000)
|
||||
{
|
||||
// Store LLA
|
||||
/* Store LLA */
|
||||
home.Latitude = gpsData->Latitude;
|
||||
home.Longitude = gpsData->Longitude;
|
||||
home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation;
|
||||
|
||||
// Compute home ECEF coordinates and the rotation matrix into NED
|
||||
double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) };
|
||||
/* Compute home ECEF coordinates and the rotation matrix into NED
|
||||
* Note that floats are used here - they should give enough precision
|
||||
* for this application.*/
|
||||
|
||||
// Compute magnetic flux direction at home location
|
||||
if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0)
|
||||
{ // calculations appeared to go OK
|
||||
float LLA[3] = { (home.Latitude)/10e6f, (home.Longitude)/10e6f, (home.Altitude) };
|
||||
|
||||
// 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.
|
||||
/* Compute magnetic flux direction at home location */
|
||||
if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) {
|
||||
|
||||
/*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.Set = HOMELOCATION_SET_TRUE;
|
||||
HomeLocationSet(&home);
|
||||
|
||||
@ -353,7 +353,7 @@ static void manualControlTask(void *parameters)
|
||||
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
|
||||
|
||||
// Apply deadband for Roll/Pitch/Yaw stick inputs
|
||||
if (settings.Deadband) {
|
||||
if (settings.Deadband > 0.0f) {
|
||||
applyDeadband(&cmd.Roll, settings.Deadband);
|
||||
applyDeadband(&cmd.Pitch, 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 portTickType lastSysTime;
|
||||
portTickType thisSysTime = xTaskGetTickCount();
|
||||
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
if (home && changed) {
|
||||
// Simple Return To Base mode - keep altitude the same, fly to home position
|
||||
PositionActualData positionActual;
|
||||
@ -754,8 +749,8 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
|
||||
*/
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
|
||||
{
|
||||
const float DEADBAND_HIGH = 0.55;
|
||||
const float DEADBAND_LOW = 0.45;
|
||||
const float DEADBAND_HIGH = 0.55f;
|
||||
const float DEADBAND_LOW = 0.45f;
|
||||
|
||||
static portTickType lastSysTime;
|
||||
static bool zeroed = false;
|
||||
@ -768,7 +763,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
|
||||
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f);
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
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
|
||||
if (valueScaled > 1.0) valueScaled = 1.0;
|
||||
else
|
||||
if (valueScaled < -1.0) valueScaled = -1.0;
|
||||
if (valueScaled > 1.0f) {
|
||||
valueScaled = 1.0f;
|
||||
} else if (valueScaled < -1.0f) {
|
||||
valueScaled = -1.0f;
|
||||
}
|
||||
|
||||
return valueScaled;
|
||||
}
|
||||
|
||||
@ -80,7 +80,6 @@ MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart)
|
||||
|
||||
static void WavPlayerTask(void *parameters)
|
||||
{
|
||||
portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
portTickType lastSysTime;
|
||||
// Loop forever
|
||||
lastSysTime = xTaskGetTickCount(); //portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
|
||||
@ -292,7 +292,7 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
|
||||
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)
|
||||
{
|
||||
@ -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 y0 = (size/2)+dy;
|
||||
// calculate the line function
|
||||
if((angle != 90) && (angle != -90))
|
||||
if((angle < 90.0f) && (angle > -90.0f))
|
||||
{
|
||||
k = tanf(alpha);
|
||||
vertical = 0;
|
||||
if(k==0)
|
||||
{
|
||||
horizontal=1;
|
||||
if(fabsf(angle) < 1e-5f) {
|
||||
horizontal = 1;
|
||||
} else {
|
||||
k = tanf(alpha);
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -1960,7 +1960,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
|
||||
// horizon line
|
||||
write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
|
||||
//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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
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
|
||||
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);
|
||||
for(int i=0;i<size;i++)
|
||||
@ -2143,7 +2143,7 @@ void calcHomeArrow(int16_t m_yaw)
|
||||
d = 6371 * 1000 * c;
|
||||
|
||||
// Elevation v depends servo direction
|
||||
if(d!=0)
|
||||
if(d > 0.0f)
|
||||
elevation = 90-RAD2DEG(atanf(dAlt/d));
|
||||
else
|
||||
elevation = 0;
|
||||
|
||||
@ -509,7 +509,9 @@ static void magOffsetEstimation(MagnetometerData *mag)
|
||||
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
|
||||
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.y += delta[1];
|
||||
magBias.z += delta[2];
|
||||
|
||||
@ -60,8 +60,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
|
||||
static uint32_t accumulated = 0;
|
||||
|
||||
const uint16_t DEGLITCH_TIME = 20; // ms
|
||||
const float AMPLITUDE_ALPHA = 0.95;
|
||||
const float PERIOD_ALPHA = 0.95;
|
||||
const float AMPLITUDE_ALPHA = 0.95f;
|
||||
const float PERIOD_ALPHA = 0.95f;
|
||||
|
||||
portTickType thisTime = xTaskGetTickCount();
|
||||
|
||||
|
||||
@ -248,7 +248,7 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
// Compute the inner loop
|
||||
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;
|
||||
|
||||
@ -264,7 +264,7 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
// Compute the inner loop
|
||||
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;
|
||||
|
||||
@ -288,7 +288,7 @@ static void stabilizationTask(void* parameters)
|
||||
// Compute desired rate as input biased towards 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] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
}
|
||||
@ -310,7 +310,7 @@ static void stabilizationTask(void* parameters)
|
||||
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] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
@ -320,7 +320,7 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
// Run the relay controller which also estimates the oscillation parameters
|
||||
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;
|
||||
|
||||
@ -334,12 +334,12 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
// Run the relay controller which also estimates the oscillation parameters
|
||||
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;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f);
|
||||
break;
|
||||
default:
|
||||
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
|
||||
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
|
||||
// calculation
|
||||
const float fakeDt = 0.0025;
|
||||
const float fakeDt = 0.0025f;
|
||||
if(settings.GyroTau < 0.0001f)
|
||||
gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
else
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include <pios_math.h>
|
||||
#include "stabilization.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)
|
||||
{
|
||||
const float F_PI = (float) M_PI;
|
||||
float cy = cosf(z_gyro / 180.0f * F_PI * dT);
|
||||
float sy = sinf(z_gyro / 180.0f * F_PI * dT);
|
||||
float cy = cosf(DEG2RAD(z_gyro) * dT);
|
||||
float sy = sinf(DEG2RAD(z_gyro) * dT);
|
||||
|
||||
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
|
||||
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
|
||||
|
||||
@ -461,9 +461,9 @@ static void updateStats()
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
|
||||
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;
|
||||
const float STM32_TEMP_V25 = 1.43f; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
|
||||
#endif
|
||||
SystemStatsSet(&stats);
|
||||
}
|
||||
|
||||
@ -437,8 +437,8 @@ static void updateTelemetryStats()
|
||||
|
||||
// Update stats object
|
||||
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
|
||||
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
|
||||
flightStats.TxDataRate = (float)utalkStats.txBytes / ((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.0f);
|
||||
flightStats.RxFailures += utalkStats.rxErrors;
|
||||
flightStats.TxFailures += txErrors;
|
||||
flightStats.TxRetries += txRetries;
|
||||
|
||||
@ -178,7 +178,7 @@ static void updatePIDs(UAVObjEvent* ev)
|
||||
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
|
||||
inst.MinPID[i], inst.MaxPID[i]);
|
||||
} 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 {
|
||||
continue;
|
||||
}
|
||||
@ -302,7 +302,7 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM
|
||||
|
||||
// normalize input value to [0..1]
|
||||
if (inMax <= inMin)
|
||||
val = 0.0;
|
||||
val = 0.0f;
|
||||
else
|
||||
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;
|
||||
outMin = outMax;
|
||||
outMax = t;
|
||||
val = 1.0 - val;
|
||||
val = 1.0f - val;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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;
|
||||
return 1;
|
||||
}
|
||||
|
||||
@ -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)
|
||||
{
|
||||
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
|
||||
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_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
|
||||
@ -1330,11 +1330,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
|
||||
} else {
|
||||
hbsel = 1;
|
||||
}
|
||||
float freq_mhz = (float)(frequency_hz) / 1000000.0;
|
||||
float xtal_freq_khz = 30000;
|
||||
float sfreq = freq_mhz / (10.0 * (xtal_freq_khz / 30000.0) * (1 + hbsel));
|
||||
float freq_mhz = (float)(frequency_hz) / 1000000.0f;
|
||||
float xtal_freq_khz = 30000.0f;
|
||||
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 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 fcl = fc & 0xff;
|
||||
|
||||
|
||||
@ -58,6 +58,12 @@
|
||||
#include <pios_sys.h>
|
||||
#include <pios_delay.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_sdcard.h>
|
||||
#include <pios_udp.h>
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
|
||||
|
||||
/* Project Includes */
|
||||
#include "pios.h"
|
||||
#include <pios.h>
|
||||
|
||||
#if defined(PIOS_INCLUDE_SYS)
|
||||
|
||||
|
||||
@ -80,7 +80,7 @@ uint32_t PIOS_RTC_Counter()
|
||||
*/
|
||||
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()
|
||||
|
||||
@ -1986,7 +1986,7 @@ void USB_OTG_ActiveRemoteWakeup(USB_OTG_CORE_HANDLE *pdev)
|
||||
if(pdev->cfg.low_power)
|
||||
{
|
||||
/* 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.stoppclk = 0;
|
||||
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)
|
||||
{
|
||||
/* 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.stoppclk = 0;
|
||||
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
|
||||
|
||||
@ -264,7 +264,7 @@ uint32_t DCD_EP_Tx ( USB_OTG_CORE_HANDLE *pdev,
|
||||
/* Setup and start the Transfer */
|
||||
ep->is_in = 1;
|
||||
ep->num = ep_addr & 0x7F;
|
||||
ep->xfer_buff = pbuf;
|
||||
ep->xfer_buff = (uint8_t*)pbuf;
|
||||
ep->dma_addr = (uint32_t)pbuf;
|
||||
ep->xfer_count = 0;
|
||||
ep->xfer_len = buf_len;
|
||||
|
||||
@ -358,7 +358,7 @@ static uint32_t DCD_HandleResume_ISR(USB_OTG_CORE_HANDLE *pdev)
|
||||
if(pdev->cfg.low_power)
|
||||
{
|
||||
/* 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.stoppclk = 0;
|
||||
USB_OTG_WRITE_REG32(pdev->regs.PCGCCTL, power.d32);
|
||||
|
||||
@ -668,7 +668,7 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
|
||||
* failures at this transition which previously resulted
|
||||
* 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--)
|
||||
continue;
|
||||
if (!guard) {
|
||||
|
||||
@ -88,7 +88,7 @@ uint32_t PIOS_RTC_Counter()
|
||||
*/
|
||||
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()
|
||||
|
||||
@ -210,7 +210,7 @@ extern uint32_t pios_com_debug_id;
|
||||
// Currently analog acquistion hard coded at 480 Hz
|
||||
// PCKL2 = HCLK / 16
|
||||
// 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
|
||||
|
||||
//------------------------
|
||||
|
||||
@ -209,9 +209,9 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info)
|
||||
}
|
||||
else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 )
|
||||
{
|
||||
initfields.append( QString("\tdata.%1 = %2;\r\n")
|
||||
.arg( info->fields[n]->name )
|
||||
.arg( info->fields[n]->defaultValues[0].toFloat() ) );
|
||||
initfields.append( QString("\tdata.%1 = %2f;\r\n")
|
||||
.arg(info->fields[n]->name)
|
||||
.arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6));
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -234,10 +234,10 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info)
|
||||
}
|
||||
else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 )
|
||||
{
|
||||
initfields.append( QString("\tdata.%1[%2] = %3;\r\n")
|
||||
.arg( info->fields[n]->name )
|
||||
.arg( idx )
|
||||
.arg( info->fields[n]->defaultValues[idx].toFloat() ) );
|
||||
initfields.append( QString("\tdata.%1[%2] = %3f;\r\n")
|
||||
.arg(info->fields[n]->name)
|
||||
.arg(idx)
|
||||
.arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6));
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@ -119,14 +119,11 @@ CFLAGS += -g$(DEBUGF)
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Wfloat-equal -Wunsuffixed-float-constants
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
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)
|
||||
CFLAGS += -DDEBUG
|
||||
else
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user