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

OP-932: Fixes a bug that was preventing setting of Tx power on remote modem.

Browse Source
This commit is contained in:
Brian Webb 2013-05-06 01:03:15 +01:00
commit ae8286e99a
131 changed files with 1236 additions and 864 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 ************
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;

240
flight/libraries/WorldMagModel.c
View File

@ -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 (fabsf(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++)

4
flight/libraries/inc/insgps.h
View File

@ -59,11 +59,13 @@ void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSGetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetPosVelVar(float PosVar[3], float VelVar[3]);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var);

35
flight/libraries/insgps13state.c
View File

@ -141,7 +141,19 @@ void INSResetP(float PDiag[NUMX])
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
void INSGetP(float PDiag[NUMX])
{
uint8_t i;
// retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){
if (PDiag != 0){
PDiag[i] = P[i][i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
@ -179,14 +191,14 @@ void INSPosVelReset(float pos[3], float vel[3])
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar, float VelVar)
void INSSetPosVelVar(float PosVar[3], float VelVar[3])
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
R[5] = VelVar;
R[0] = PosVar[0];
R[1] = PosVar[1];
R[2] = PosVar[2];
R[3] = VelVar[0];
R[4] = VelVar[1];
R[5] = VelVar[2];
}
void INSSetGyroBias(float gyro_bias[3])
@ -210,6 +222,13 @@ void INSSetGyroVar(float gyro_var[3])
Q[2] = gyro_var[2];
}
void INSSetGyroBiasVar(float gyro_bias_var[3])
{
Q[6] = gyro_bias_var[0];
Q[7] = gyro_bias_var[1];
Q[8] = gyro_bias_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];

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;
//! 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)

2
flight/libraries/op_dfu.c
View File

@ -78,7 +78,7 @@ extern uint8_t JumpToApp;
void sendData(uint8_t * buf, uint16_t size);
uint32_t CalcFirmCRC(void);
void DataDownload(DownloadAction action) {
void DataDownload(__attribute__((unused)) DownloadAction action) {
if ((DeviceState == downloading)) {
uint8_t packetSize;

16
flight/libraries/paths.c
View File

@ -24,7 +24,9 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#include <pios.h>
#include <pios_math.h>
#include "paths.h"
#include "uavobjectmanager.h" // <--.
@ -216,18 +218,18 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
a_radius = atan2f(radius_north, radius_east);
if (a_diff < 0) {
a_diff += 2 * M_PI;
a_diff += 2.0f * M_PI_F;
}
if (a_radius < 0) {
a_radius += 2 * M_PI;
a_radius += 2.0f * M_PI_F;
}
progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
if (progress < 0) {
progress += 1.0;
} else if (progress >= 1) {
progress -= 1.0;
progress += 1.0f;
} else if (progress >= 1.0f) {
progress -= 1.0f;
}
if (clockwise) {

52
flight/libraries/printf2.c
View File

@ -74,7 +74,7 @@ char **environ = __env;
/*==============================================================================
* Close a file.
*/
int _close(int file)
int _close(__attribute__((unused)) int file)
{
return -1;
}
@ -82,7 +82,7 @@ int _close(int file)
/*==============================================================================
* Transfer control to a new process.
*/
int _execve(char *name, char **argv, char **env)
int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env)
{
errno = ENOMEM;
return -1;
@ -91,7 +91,7 @@ int _execve(char *name, char **argv, char **env)
/*==============================================================================
* Exit a program without cleaning up files.
*/
void _exit( int code )
void _exit( __attribute__((unused)) int code )
{
/* Should we force a system reset? */
while( 1 )
@ -112,7 +112,7 @@ int _fork(void)
/*==============================================================================
* Status of an open file.
*/
int _fstat(int file, struct stat *st)
int _fstat(__attribute__((unused)) int file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
@ -129,7 +129,7 @@ int _getpid(void)
/*==============================================================================
* Query whether output stream is a terminal.
*/
int _isatty(int file)
int _isatty(__attribute__((unused)) int file)
{
return 1;
}
@ -137,7 +137,7 @@ int _isatty(int file)
/*==============================================================================
* Send a signal.
*/
int _kill(int pid, int sig)
int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
{
errno = EINVAL;
return -1;
@ -146,7 +146,7 @@ int _kill(int pid, int sig)
/*==============================================================================
* Establish a new name for an existing file.
*/
int _link(char *old, char *new)
int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
{
errno = EMLINK;
return -1;
@ -155,7 +155,7 @@ int _link(char *old, char *new)
/*==============================================================================
* Set position in a file.
*/
int _lseek(int file, int ptr, int dir)
int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
{
return 0;
}
@ -163,7 +163,7 @@ int _lseek(int file, int ptr, int dir)
/*==============================================================================
* Open a file.
*/
int _open(const char *name, int flags, int mode)
int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
{
return -1;
}
@ -171,18 +171,18 @@ int _open(const char *name, int flags, int mode)
/*==============================================================================
* Read from a file.
*/
int _read(int file, char *ptr, int len)
int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
{
return 0;
}
/*==============================================================================
* 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.
*/
int _write_r( void * reent, int file, char * ptr, int len )
int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len )
{
return 0;
}
@ -220,7 +220,7 @@ caddr_t _sbrk(int incr)
/*==============================================================================
* Status of a file (by name).
*/
int _stat(char *file, struct stat *st)
int _stat(__attribute__((unused)) char *file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
@ -229,7 +229,7 @@ int _stat(char *file, struct stat *st)
/*==============================================================================
* Timing information for current process.
*/
int _times(struct tms *buf)
int _times(__attribute__((unused)) struct tms *buf)
{
return -1;
}
@ -237,7 +237,7 @@ int _times(struct tms *buf)
/*==============================================================================
* Remove a file's directory entry.
*/
int _unlink(char *name)
int _unlink(__attribute__((unused)) char *name)
{
errno = ENOENT;
return -1;
@ -246,7 +246,7 @@ int _unlink(char *name)
/*==============================================================================
* Wait for a child process.
*/
int _wait(int *status)
int _wait(__attribute__((unused)) int *status)
{
errno = ECHILD;
return -1;
@ -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++ = '+' ;

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

@ -150,12 +150,13 @@ MODULE_INITCALL(ActuatorInitialize, ActuatorStart)
*
* @return -1 if error, 0 if success
*/
static void actuatorTask(void* parameters)
static void actuatorTask(__attribute__((unused)) void* parameters)
{
UAVObjEvent ev;
portTickType lastSysTime;
portTickType thisSysTime;
float dT = 0.0f;
float dTSeconds;
uint32_t dTMilliseconds;
ActuatorCommandData command;
ActuatorDesiredData desired;
@ -206,13 +207,9 @@ static void actuatorTask(void* parameters)
// Check how long since last update
thisSysTime = xTaskGetTickCount();
// reuse dt in case of wraparound
// todo:
// if dT actually matters...
// fix it to know max value and subtract for currently correct dT on wrap
if(thisSysTime > lastSysTime)
dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f);
dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS;
lastSysTime = thisSysTime;
dTSeconds = dTMilliseconds * 0.001f;
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
@ -296,7 +293,7 @@ static void actuatorTask(void* parameters)
}
if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
else
status[ct] = -1;
@ -370,7 +367,7 @@ static void actuatorTask(void* parameters)
actuatorSettings.ChannelNeutral[i]);
// Store update time
command.UpdateTime = dT * 1000.0f;
command.UpdateTime = dTMilliseconds;
if (command.UpdateTime > command.MaxUpdateTime)
command.MaxUpdateTime = command.UpdateTime;
@ -431,7 +428,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)
{
@ -721,12 +718,12 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuato
}
}
static void ActuatorSettingsUpdatedCb(UAVObjEvent * ev)
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
actuator_settings_updated = true;
}
static void MixerSettingsUpdatedCb(UAVObjEvent * ev)
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
mixer_settings_updated = true;
}

4
flight/modules/Airspeed/revolution/airspeed.c
View File

@ -129,7 +129,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart)
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
static void airspeedTask(__attribute__((unused)) void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
@ -177,7 +177,7 @@ static void airspeedTask(void *parameters)
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedSettingsGet(&airspeedSettings);

2
flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c
View File

@ -61,7 +61,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
//Check to see if airspeed sensor is returning airspeedSensor
airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (airspeedSensor->SensorValue==-1) {
if (airspeedSensor->SensorValue==(uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
return;

2
flight/modules/Altitude/altitude.c
View File

@ -125,7 +125,7 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
* Module thread, should not return.
*/
static void altitudeTask(void *parameters)
static void altitudeTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;

2
flight/modules/Altitude/revolution/altitude.c
View File

@ -84,7 +84,7 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
* Module thread, should not return.
*/
static void altitudeTask(void *parameters)
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroAltitudeData data;

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

@ -118,7 +118,7 @@ float starting_altitude;
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
static void altitudeHoldTask(__attribute__((unused)) void *parameters)
{
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
@ -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};
@ -383,7 +386,7 @@ static void altitudeHoldTask(void *parameters)
}
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
}

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

@ -93,7 +93,7 @@ static float accels_filtered[3];
static float grot_filtered[3];
static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
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];
@ -174,7 +174,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
int32_t accel_test;
int32_t gyro_test;
static void AttitudeTask(void *parameters)
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@ -218,17 +218,17 @@ static void AttitudeTask(void *parameters)
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// Use accels to initialise attitude and calculate gyro bias
accelKp = 1;
accelKp = 1.0f;
accelKi = 0.0f;
yawBiasRate = 0.01;
yawBiasRate = 0.01f;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKp = 1.0f;
accelKi = 0.0f;
yawBiasRate = 0.01;
yawBiasRate = 0.01f;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
@ -469,7 +469,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 : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
@ -547,18 +547,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;
@ -572,7 +573,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
AttitudeActualSet(&attitudeActual);
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
@ -583,7 +584,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;

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

@ -66,6 +66,8 @@
#include "homelocation.h"
#include "magnetometer.h"
#include "positionactual.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocityactual.h"
@ -102,10 +104,15 @@ static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings;
static bool gyroBiasSettingsUpdated = false;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
// Private functions
static void AttitudeTask(void *parameters);
@ -115,6 +122,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
// check for invalid values
static inline bool invalid(float data) {
if ( isnan(data) || isinf(data) ){
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data) {
if ( invalid(data) ) {
return true;
}
if ( data < 1e-15f ) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
@ -138,6 +164,9 @@ int32_t AttitudeInitialize(void)
VelocityActualInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
@ -145,24 +174,26 @@ int32_t AttitudeInitialize(void)
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0;
gyrosBias.y = 0;
gyrosBias.z = 0;
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -203,10 +234,9 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
bool first_run = true;
uint32_t last_algorithm;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
@ -215,21 +245,19 @@ static void AttitudeTask(void *parameters)
// Wait for all the sensors be to read
vTaskDelay(100);
// Invalidate previous algorithm to trigger a first run
last_algorithm = 0xfffffff;
// Main task loop
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) {
last_algorithm = revoSettings.FusionAlgorithm;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) {
switch (running_algorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
@ -244,8 +272,9 @@ static void AttitudeTask(void *parameters)
break;
}
if(ret_val == 0)
first_run = false;
if(ret_val != 0) {
initialization_required = true;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
@ -282,8 +311,6 @@ static int32_t updateAttitudeComplementary(bool first_run)
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
@ -293,16 +320,36 @@ static int32_t updateAttitudeComplementary(bool first_run)
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
@ -315,15 +362,15 @@ 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;
magKp = 1;
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
@ -350,8 +397,8 @@ static int32_t updateAttitudeComplementary(bool first_run)
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
@ -373,7 +420,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);
@ -387,13 +434,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
if (bmag < 1.0f || mag_len < 1.0f)
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
@ -404,6 +451,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
@ -412,10 +466,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
@ -423,7 +477,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
if(q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
@ -439,11 +493,11 @@ 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)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
}
quat_copy(q, &attitudeActual.q1);
@ -503,7 +557,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0;
}
@ -537,7 +596,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
static bool gps_updated;
static bool gps_vel_updated;
static float baroOffset = 0;
static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0;
static bool inited;
@ -611,119 +672,206 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GPSVelocityGet(&gpsVelData);
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);
value_error = false;
// safety checks
if ( invalid(gyrosData.x) ||
invalid(gyrosData.y) ||
invalid(gyrosData.z) ||
invalid(accelsData.x) ||
invalid(accelsData.y) ||
invalid(accelsData.z) ) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if ( invalid(gyrosBias.x) ||
invalid(gyrosBias.y) ||
invalid(gyrosBias.z) ) {
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
}
if ( invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z) ) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// 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]);
if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) {
mag_updated = false;
value_error = true;
}
if ( invalid(baroData.Altitude) ) {
baro_updated = false;
value_error = true;
}
if ( invalid(airspeedData.CalibratedAirspeed) ) {
airspeed_updated = false;
value_error = true;
}
if ( invalid(gpsData.Altitude) ) {
gps_updated = false;
value_error = true;
}
if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) {
gps_updated = false;
value_error = true;
}
if ( invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down) ) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
if ( ( gpsData.Satellites < 7 ) ||
( gpsData.PDOP > 4.0f ) ||
( gpsData.Latitude==0 && gpsData.Longitude==0 ) ||
( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited)
if ( !inited ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else if (outdoor_mode && gpsData.Satellites < 7)
} else if ( value_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if ( variance_error ) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f) {
dT = 0.01f;
} else if(dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read
if (init_stage == 0 && !outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
if (init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } );
INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } );
INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } );
INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } );
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias
float gyro_bias[3] = {0.0f, 0.0f, 0.0f};
INSSetGyroBias(gyro_bias);
float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
if (outdoor_mode) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
AttitudeActualGet (&attitudeActual);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float NED[3];
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Initialize barometric offset to cirrent GPS NED coordinate
baroOffset = -NED[2] - baroData.Altitude;
INSResetP(ekfConfiguration.P);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(NED, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage > 0) {
} else {
// Run prediction a bit before any corrections
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f,
(gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f,
(gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f};
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
@ -738,38 +886,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if(init_stage > 10)
inited = true;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
if (!inited)
return 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f)
dT = 0.01f;
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * M_PI_F / 180.0f;
gyros[1] += gyrosBias.y * M_PI_F / 180.0f;
gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
// Advance the state estimate
@ -798,14 +927,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (gps_updated && outdoor_mode)
{
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] });
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0;
vel[2] = 0.0f;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
@ -816,9 +950,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) {
INSSetPosVelVar(1e6f, 1e5f);
vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0;
INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] });
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS |VERT_SENSORS;
@ -843,11 +982,16 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if ( !gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar(1e6f, 1e2f);
INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] });
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q,R);
float vtas[3] = {airspeed.TrueAirspeed,0,0};
float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f};
rot_mult(R,vtas,vel);
}
}
@ -874,15 +1018,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
GyrosBiasSet(&gyrosBias);
}
gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]);
gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]);
gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]);
GyrosBiasSet(&gyrosBias);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(vardata.P);
EKFStateVarianceSet(&vardata);
return 0;
}
@ -912,24 +1056,58 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
static void settingsUpdatedCb(UAVObjEvent * ev)
{
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
/* When the revo calibration is updated, update the GyroBias object */
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
GyrosBiasSet(&gyrosBias);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL ||
ev->obj == EKFConfigurationHandle() ||
ev->obj == RevoSettingsHandle() ||
( variance_error==true && ev->obj == FlightStatusHandle() )
) {
gyroBiasSettingsUpdated = true;
bool error = false;
// In case INS currently running
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
EKFConfigurationGet(&ekfConfiguration);
int t;
for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.P[t])) {
error = true;
}
}
for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q[t])) {
error = true;
}
}
for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) {
error = true;
}
if (error) {
variance_error = true;
} else {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if(ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
@ -941,11 +1119,12 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionActual to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle())
AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
}
/**
* @}

2
flight/modules/Autotune/autotune.c
View File

@ -120,7 +120,7 @@ MODULE_INITCALL(AutotuneInitialize, AutotuneStart)
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
static void AutotuneTask(__attribute__((unused)) void *parameters)
{
//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

2
flight/modules/Battery/battery.c
View File

@ -124,7 +124,7 @@ int32_t BatteryInitialize(void)
MODULE_INITCALL(BatteryInitialize, 0)
#define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED
static void onTimer(UAVObjEvent* ev)
static void onTimer(__attribute__((unused)) UAVObjEvent* ev)
{
static FlightBatteryStateData flightBatteryData;
FlightBatterySettingsData batterySettings;

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

@ -180,7 +180,7 @@ static void attitudeUpdated(UAVObjEvent* ev)
break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal * cameraStab.InputRate[i];
if (fabs(input_rate) > cameraStab.MaxAxisLockRate)
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate)
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]);
break;
default:
@ -328,7 +328,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
float delta = *attitude - csd->ffLastAttitudeFiltered[index];
float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis;
if (fabs(delta) > maxDelta) {
if (fabsf(delta) > maxDelta) {
// we are accelerating too hard
*attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta);
}

8
flight/modules/ComUsbBridge/ComUsbBridge.c
View File

@ -126,7 +126,7 @@ MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart)
* Main task. It does not return.
*/
static void com2UsbBridgeTask(void *parameters)
static void com2UsbBridgeTask(__attribute__((unused)) void *parameters)
{
/* Handle usart -> vcp direction */
volatile uint32_t tx_errors = 0;
@ -136,7 +136,7 @@ static void com2UsbBridgeTask(void *parameters)
rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != rx_bytes) {
if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) {
/* Error on transmit */
tx_errors++;
}
@ -144,7 +144,7 @@ static void com2UsbBridgeTask(void *parameters)
}
}
static void usb2ComBridgeTask(void * parameters)
static void usb2ComBridgeTask(__attribute__((unused)) void * parameters)
{
/* Handle vcp -> usart direction */
volatile uint32_t tx_errors = 0;
@ -154,7 +154,7 @@ static void usb2ComBridgeTask(void * parameters)
rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != rx_bytes) {
if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) {
/* Error on transmit */
tx_errors++;
}

2
flight/modules/Example/examplemodcallback.c
View File

@ -83,7 +83,7 @@ int32_t ExampleModCallbackInitialize()
* event thread. Because of that the callback execution time must be kept to
* a minimum.
*/
static void ObjectUpdatedCb(UAVObjEvent * ev)
static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(cbinfo);
}

2
flight/modules/Example/examplemodperiodic.c
View File

@ -76,7 +76,7 @@ int32_t ExampleModPeriodicInitialize()
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
static void exampleTask(__attribute__((unused)) void *parameters)
{
ExampleSettingsData settings;
ExampleObject2Data data;

2
flight/modules/Example/examplemodthread.c
View File

@ -84,7 +84,7 @@ int32_t ExampleModThreadInitialize()
/**
* Module thread, should not return.
*/
static void exampleTask(void *parameters)
static void exampleTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
ExampleSettingsData settings;

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

@ -140,7 +140,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
};
#endif
static void magbaroTask(void *parameters)
static void magbaroTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
@ -185,7 +185,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
@ -193,7 +193,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);

2
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -238,7 +238,7 @@ static uint32_t get_time(void)
/**
* Executed by event dispatcher callback to reset INS before resetting OP
*/
static void resetTask(UAVObjEvent * ev)
static void resetTask(__attribute__((unused)) UAVObjEvent * ev)
{
#if defined (PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);

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

@ -143,7 +143,7 @@ static float indicatedAirspeedActualBias = 0;
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
static void pathfollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
@ -315,7 +315,7 @@ static void updatePathVelocity()
if (angle1>180.0f) angle1-=360.0f;
if (angle2<-180.0f) angle2+=360.0f;
if (angle2>180.0f) angle2-=360.0f;
if (fabs(angle1)>=90.0f && fabs(angle2)>=90.0f) {
if (fabsf(angle1)>=90.0f && fabsf(angle2)>=90.0f) {
error_speed=0;
}
@ -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
*/
@ -639,13 +629,13 @@ static float bound(float val, float min, float max)
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
}
static void airspeedActualUpdatedCb(UAVObjEvent * ev)
static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedActualData airspeedActual;

2
flight/modules/FlightPlan/flightplan.c
View File

@ -94,7 +94,7 @@ MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart)
/**
* Module task
*/
static void flightPlanTask(void *parameters)
static void flightPlanTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
PmReturn_t retval;

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

@ -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
// ****************
/**
@ -192,7 +194,7 @@ MODULE_INITCALL(GPSInitialize, GPSStart)
* Main gps task. It does not return.
*/
static void gpsTask(void *parameters)
static void gpsTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
@ -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
@ -273,16 +275,16 @@ static void gpsTask(void *parameters)
/*
* Estimate the acceleration due to gravity for a particular location in LLA
*/
static float GravityAccel(float latitude, float longitude, float altitude)
static float GravityAccel(float latitude, __attribute__((unused)) 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);

8
flight/modules/GPS/NMEA.c
View File

@ -78,7 +78,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
#endif //PIOS_GPS_MINIMAL
const static struct nmea_parser nmea_parsers[] = {
static const struct nmea_parser nmea_parsers[] = {
{
.prefix = "GPGGA",
.handler = nmeaProcessGPGGA,
@ -189,7 +189,7 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData,
return PARSER_INCOMPLETE;
}
const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
{
if (!prefix) {
return (NULL);
@ -601,7 +601,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
* \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
{
if (nbParam != 7)
return false;
@ -640,7 +640,7 @@ static uint8_t gsv_processed_mask;
static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error;
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
{
if (nbParam < 4)
return false;

2
flight/modules/MK/MKSerial/MKSerial.c
View File

@ -501,7 +501,7 @@ static void DoConnectedToNC(void)
}
}
static void MkSerialTask(void *parameters)
static void MkSerialTask(__attribute__((unused)) void *parameters)
{
MkMsg_t msg;
uint32_t version;

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

@ -73,7 +73,11 @@
// Private types
typedef enum
{
ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
@ -160,7 +164,7 @@ MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
/**
* Module task
*/
static void manualControlTask(void *parameters)
static void manualControlTask(__attribute__((unused)) void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
@ -246,7 +250,7 @@ static void manualControlTask(void *parameters)
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT)
valid_input_detected = false;
else
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
@ -351,7 +355,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,12 +700,14 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed,bool home)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
@ -749,15 +755,17 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
}
}
static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime;
float dT;
thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
PositionActualData positionActual;
PositionActualGet(&positionActual);
@ -787,8 +795,8 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
*/
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;
@ -800,9 +808,9 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
@ -830,17 +838,21 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening
// in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed,
__attribute__((unused)) bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData * cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
@ -868,21 +880,17 @@ 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;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
if (end_time > start_time) {
return (end_time - start_time) * portTICK_RATE_MS;
}
return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS;
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
@ -898,9 +906,9 @@ static bool okToArm(void)
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
@ -1106,7 +1114,7 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
*/
static void applyDeadband(float *value, float deadband)
{
if (fabs(*value) < deadband)
if (fabsf(*value) < deadband)
*value = 0.0f;
else if (*value > 0.0f)
*value -= deadband;
@ -1130,7 +1138,7 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
/**
* Called whenever a critical configuration component changes
*/
static void configurationUpdatedCb(UAVObjEvent * ev)
static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
configuration_check();
}

5
flight/modules/OPLink/oplinkmod.c
View File

@ -120,7 +120,7 @@ MODULE_INITCALL(OPLinkModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
static void systemTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
uint16_t prev_tx_count = 0;
@ -224,7 +224,8 @@ void vApplicationIdleHook(void)
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask,
__attribute__((unused)) signed portCHAR * pcTaskName)
{
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW

6
flight/modules/Osd/OsdEtStd/OsdEtStd.c
View File

@ -206,17 +206,17 @@ static void SetNbSats(uint8_t nb)
msg[OSDMSG_NB_SATS] = nb;
}
static void FlightBatteryStateUpdatedCb(UAVObjEvent * ev)
static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newBattData = TRUE;
}
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newPosData = TRUE;
}
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newBaroData = TRUE;
}

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

@ -76,9 +76,8 @@ MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart)
* Main WavPlayer task. It does not return.
*/
static void WavPlayerTask(void *parameters)
static void WavPlayerTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
portTickType lastSysTime;
// Loop forever
lastSysTime = xTaskGetTickCount();

14
flight/modules/Osd/osdgen/inc/osdgen.h
View File

@ -129,14 +129,14 @@ struct FontDimensions
#define APPLY_VDEADBAND(y) ((y)+GRAPHICS_VDEADBAND)
#define APPLY_HDEADBAND(x) ((x)+GRAPHICS_HDEADBAND)
// Check if coordinates are valid. If not, return.
#define CHECK_COORDS(x, y) if(x < 0 || x >= GRAPHICS_WIDTH_REAL || y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
#define CHECK_COORD_X(x) if(x < 0 || x >= GRAPHICS_WIDTH_REAL) return;
#define CHECK_COORD_Y(y) if(y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
// Check if coordinates are valid. If not, return. Assumes unsigned coordinate
#define CHECK_COORDS(x, y) if(x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) return;
#define CHECK_COORD_X(x) if(x >= GRAPHICS_WIDTH_REAL) return;
#define CHECK_COORD_Y(y) if(y >= GRAPHICS_HEIGHT_REAL) return;
// Clip coordinates out of range.
#define CLIP_COORD_X(x) { x = MAX(0, MIN(x, GRAPHICS_WIDTH_REAL)); }
#define CLIP_COORD_Y(y) { y = MAX(0, MIN(y, GRAPHICS_HEIGHT_REAL)); }
// Clip coordinates out of range - assumes unsigned coordinate
#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); }
#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); }
#define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); }
// Macro to swap two variables using XOR swap.

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

@ -98,12 +98,19 @@ struct splashEntry
const uint16_t *mask;
};
struct splashEntry splash[3] =
{
{ oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits },
{ level_width, level_height, level_bits, level_mask_bits },
{ llama_width, llama_height, llama_bits, llama_mask_bits },
struct splashEntry splash[3] = {
{ oplogo_width,
oplogo_height,
oplogo_bits,
oplogo_mask_bits },
{ level_width,
level_height,
level_bits,
level_mask_bits },
{ llama_width,
llama_height,
llama_bits,
llama_mask_bits }
};
uint16_t mirror(uint16_t source)
@ -198,7 +205,7 @@ void swap(uint16_t* a, uint16_t* b)
*b = temp;
}
const static int8_t sinData[91] =
static const int8_t sinData[91] =
{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };
@ -282,8 +289,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) {
error += doubleHorizontalRadius;
@ -479,10 +485,10 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
}
/* This is an optimised algorithm for writing vertical lines.
* We begin by finding the addresses of the x,y0 and x,y1 points. */
int addr0 = CALC_BUFF_ADDR(x, y0);
int addr1 = CALC_BUFF_ADDR(x, y1);
unsigned int addr0 = CALC_BUFF_ADDR(x, y0);
unsigned int addr1 = CALC_BUFF_ADDR(x, y1);
/* Then we calculate the pixel data to be written. */
int bitnum = CALC_BIT_IN_WORD(x);
unsigned int bitnum = CALC_BIT_IN_WORD(x);
uint16_t mask = 1 << (7 - bitnum);
/* Run from addr0 to addr1 placing pixels. Increment by the number
* of words n each graphics line. */
@ -552,7 +558,7 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int
*/
void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode)
{
int yy, addr0_old, addr1_old;
unsigned int yy, addr0_old, addr1_old;
CHECK_COORDS(x, y);
CHECK_COORD_X(x + width);
CHECK_COORD_Y(y + height);
@ -561,11 +567,11 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
}
// Calculate as if the rectangle was only a horizontal line. We then
// step these addresses through each row until we iterate `height` times.
int addr0 = CALC_BUFF_ADDR(x, y);
int addr1 = CALC_BUFF_ADDR(x + width, y);
int addr0_bit = CALC_BIT_IN_WORD(x);
int addr1_bit = CALC_BIT_IN_WORD(x + width);
int mask, mask_l, mask_r, i;
unsigned int addr0 = CALC_BUFF_ADDR(x, y);
unsigned int addr1 = CALC_BUFF_ADDR(x + width, y);
unsigned int addr0_bit = CALC_BIT_IN_WORD(x);
unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width);
unsigned int mask, mask_l, mask_r, i;
// If the addresses are equal, we need to write one word vertically.
if (addr0 == addr1) {
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
@ -788,7 +794,7 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode)
{
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
int steep = abs(y1 - y0) > abs(x1 - x0);
unsigned int steep = abs(y1 - y0) > abs(x1 - x0);
if (steep) {
SWAP(x0, y0);
SWAP(x1, y1);
@ -798,11 +804,11 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
SWAP(y0, y1);
}
int deltax = x1 - x0;
int deltay = abs(y1 - y0);
unsigned int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
int y = y0;
int x; //, lasty = y, stox = 0;
unsigned int y = y0;
unsigned int x; //, lasty = y, stox = 0;
if (y0 < y1) {
ystep = 1;
} else {
@ -851,7 +857,9 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i
* @param mode 0 = black outline, white body, 1 = white outline, black body
* @param mmode 0 = clear, 1 = set, 2 = toggle
*/
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
__attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1,
int mode, int mmode)
{
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
// This could be improved for speed.
@ -873,11 +881,11 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
SWAP(y0, y1);
}
int deltax = x1 - x0;
int deltay = abs(y1 - y0);
unsigned int deltay = abs(y1 - y0);
int error = deltax / 2;
int ystep;
int y = y0;
int x;
unsigned int y = y0;
unsigned int x;
if (y0 < y1) {
ystep = 1;
} else {
@ -933,8 +941,8 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
*/
void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode)
{
uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff);
int16_t firstmask = word >> xoff;
int16_t lastmask = word << (16 - xoff);
WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
if (xoff > 0) {
@ -1022,7 +1030,7 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr,
int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
{
// First locate the font struct.
if (font > SIZEOF_ARRAY(fonts)) {
if ((unsigned int)font > SIZEOF_ARRAY(fonts)) {
return 0; // font does not exist, exit.
}
// Load the font info; IDs are always sequential.
@ -1050,7 +1058,7 @@ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *loo
*/
void write_char16(char ch, unsigned int x, unsigned int y, int font)
{
int yy, addr_temp, row, row_temp, xshift;
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
struct FontEntry font_info;
//char lookup = 0;
@ -1126,14 +1134,14 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
*/
void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
{
int yy, addr_temp, row, row_temp, xshift;
unsigned int yy, addr_temp, row, row_temp, xshift;
uint16_t and_mask, or_mask, level_bits;
struct FontEntry font_info;
char lookup = 0;
fetch_font_info(ch, font, &font_info, &lookup);
// Compute starting address (for x,y) of character.
int addr = CALC_BUFF_ADDR(x, y);
int wbit = CALC_BIT_IN_WORD(x);
unsigned int addr = CALC_BUFF_ADDR(x, y);
unsigned int wbit = CALC_BIT_IN_WORD(x);
// If font only supports lowercase or uppercase, make the letter
// lowercase or uppercase.
/*if(font_info.flags & FONT_LOWERCASE_ONLY)
@ -1292,7 +1300,8 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
* @param ha horizontal align
* @param flags flags (passed to write_char)
*/
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags)
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys,
__attribute__((unused)) int va, __attribute__((unused)) int ha, int flags)
{
int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
struct FontEntry font_info;
@ -1594,7 +1603,7 @@ void printTime(uint16_t x, uint16_t y)
* @param flags special flags (see hud.h.)
*/
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
int boundtick_len, int max_val, int flags)
int boundtick_len, __attribute__((unused)) int max_val, int flags)
{
char temp[15]; //, temp2[15];
struct FontEntry font_info;
@ -1737,7 +1746,7 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
* @param majtick_len major tick length
* @param flags special flags (see hud.h.)
*/
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags)
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, __attribute__((unused)) int flags)
{
v %= 360; // wrap, just in case.
struct FontEntry font_info;
@ -1832,12 +1841,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)) {
k = tanf(alpha);
vertical = 0;
if (k == 0) {
horizontal = 1;
}
if ((angle < 90.0f) && (angle > -90)) {
vertical = 0;
if(fabsf(angle) < 1e-5f) {
horizontal = 1;
} else {
k = tanf(alpha);
}
} else {
vertical = 1;
}
@ -1875,7 +1885,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++) {
x2 = ((i - y0) + k * x0) / k;
@ -1887,7 +1897,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++) {
x2 = ((i - y0) + k * x0) / k;
@ -1899,7 +1909,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++) {
x2 = ((i - y0) + k * x0) / k;
@ -1911,7 +1921,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++) {
x2 = ((i - y0) + k * x0) / k;
@ -1927,7 +1937,7 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
} else if (vertical) {
// 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++) {
write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
@ -2017,13 +2027,13 @@ void calcHomeArrow(int16_t m_yaw)
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
brng = RAD2DEG(atan2f(y,x));
if (brng < 0) {
brng += 360;
brng += 360.0f;
}
// yaw corrected bearing, needs compass
u2g = brng - 180 - m_yaw;
u2g = brng - 180.0f - m_yaw;
if (u2g < 0) {
u2g += 360;
u2g += 360.0f;
}
// Haversine formula for distance
@ -2038,14 +2048,14 @@ void calcHomeArrow(int16_t m_yaw)
var d = R * c;
**/
a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
c = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
d = 6371 * 1000 * c;
c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a));
d = 6371.0f * 1000.0f * c;
// Elevation v depends servo direction
if (d != 0)
elevation = 90 - RAD2DEG(atanf(dAlt/d));
if(d > 0.0f)
elevation = 90.0f - RAD2DEG(atanf(dAlt/d));
else
elevation = 0;
elevation = 0.0f;
//! TODO: sanity check
char temp[50] =
@ -2117,15 +2127,17 @@ void updateGraphics()
char temp[50] =
{ 0 };
memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
// Note: cast to double required due to -Wdouble-promotion compiler option is
// being used, and there is no way in C to pass a float to a variadic function like sprintf()
sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp, "V:%5.2fV", (PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
if (gpsData.Heading > 180)
@ -2176,9 +2188,9 @@ void updateGraphics()
char temp[50] =
{ 0 };
memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Fix:%d", (int) gpsData.Status);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
@ -2197,19 +2209,19 @@ void updateGraphics()
/* Print ADC voltage */
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
sprintf(temp, "Rssi:%4.2fV", (PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print CPU temperature */
sprintf(temp, "Temp:%4.2fC", (PIOS_ADC_PinGet(3) * 0.29296875f - 264));
sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp, "FltV:%4.2fV", (PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage VIDEO*/
sprintf(temp, "VidV:%4.2fV", (PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage RSSI */
@ -2391,7 +2403,7 @@ MODULE_INITCALL( osdgenInitialize, osdgenStart)
* Main osd task. It does not return.
*/
static void osdgenTask(void *parameters)
static void osdgenTask(__attribute__((unused)) void *parameters)
{
//portTickType lastSysTime;
// Loop forever

2
flight/modules/Osd/osdinput/osdinput.c
View File

@ -110,7 +110,7 @@ MODULE_INITCALL( osdinputInitialize, osdinputStart)
* Main osdinput task. It does not return.
*/
static void osdinputTask(void *parameters)
static void osdinputTask(__attribute__((unused)) void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
portTickType lastSysTime;

2
flight/modules/Osd/osdoutput/osdoutput.c
View File

@ -136,7 +136,7 @@ static uint32_t osd_hk_com_id;
static uint8_t osd_hk_msg_dropped;
static uint8_t osd_packet;
static void send_update(UAVObjEvent * ev)
static void send_update(__attribute__((unused)) UAVObjEvent * ev)
{
static enum osd_hk_sync sync = OSD_HK_SYNC_A;

2
flight/modules/OveroSync/overosync.c
View File

@ -159,7 +159,7 @@ static void registerObject(UAVObjHandle obj)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
static void overoSyncTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;

2
flight/modules/OveroSync/simulated/overosync.c
View File

@ -158,7 +158,7 @@ static void registerObject(UAVObjHandle obj)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
static void overoSyncTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;

4
flight/modules/PathPlanner/pathplanner.c
View File

@ -115,7 +115,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
static void pathPlannerTask(void *parameters)
static void pathPlannerTask(__attribute__((unused)) void *parameters)
{
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
@ -207,7 +207,7 @@ static void pathPlannerTask(void *parameters)
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(UAVObjEvent * ev) {
void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) {
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) return;

8
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -256,7 +256,7 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
*
* @param[in] parameters The task parameters
*/
static void telemetryTxTask(void *parameters)
static void telemetryTxTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
@ -307,7 +307,7 @@ static void telemetryTxTask(void *parameters)
*
* @param[in] parameters The task parameters
*/
static void radioTxTask(void *parameters)
static void radioTxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
@ -335,7 +335,7 @@ static void radioTxTask(void *parameters)
*
* @param[in] parameters The task parameters
*/
static void radioRxTask(void *parameters)
static void radioRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
@ -364,7 +364,7 @@ static void radioRxTask(void *parameters)
*
* @param[in] parameters The task parameters
*/
static void telemetryRxTask(void *parameters)
static void telemetryRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {

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

@ -87,6 +87,8 @@ static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0};
static float gyro_staticbias[3] = {0,0,0};
static float gyro_scale[3] = {0,0,0};
static float R[3][3] = {{0}};
static int8_t rotate = 0;
@ -155,7 +157,7 @@ int32_t mag_test;
*/
uint32_t sensor_dt_us;
static void SensorsTask(void *parameters)
static void SensorsTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;
uint32_t accel_samples = 0;
@ -357,9 +359,9 @@ static void SensorsTask(void *parameters)
float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};
float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
@ -509,7 +511,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];
@ -521,7 +525,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
/**
* Locally cache some variables from the AtttitudeSettings object
*/
static void settingsUpdatedCb(UAVObjEvent * objEv) {
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@ -536,8 +540,12 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
// Do not store gyros_bias here as that comes from the state estimator and should be
// used from GyroBias directly
gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
// Zero out any adaptive tracking
MagBiasData magBias;

2
flight/modules/Sensors/simulated/sensors.c
View File

@ -144,7 +144,7 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart)
* Simulated sensor task. Run a model of the airframe and produce sensor values
*/
int sensors_count;
static void SensorsTask(void *parameters)
static void SensorsTask(__attribute__((unused)) void *parameters)
{
portTickType lastSysTime;

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

@ -56,8 +56,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();

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

@ -139,7 +139,7 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
/**
* Module task
*/
static void stabilizationTask(void* parameters)
static void stabilizationTask(__attribute__((unused)) void* parameters)
{
UAVObjEvent ev;
@ -255,7 +255,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;
@ -271,7 +271,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;
@ -296,7 +296,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;
}
@ -319,7 +319,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[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;
@ -329,7 +329,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;
@ -343,12 +343,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;
@ -432,7 +432,7 @@ static float bound(float val, float range)
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev)
{
StabilizationSettingsGet(&settings);
@ -493,7 +493,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

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

@ -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];

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

@ -155,7 +155,7 @@ MODULE_INITCALL(SystemModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
static void systemTask(__attribute__((unused))void *parameters)
{
/* start the delayed callback scheduler */
CallbackSchedulerStart();
@ -334,7 +334,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
/**
* Called whenever hardware settings changed
*/
static void hwSettingsUpdatedCb(UAVObjEvent * ev)
static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev)
{
HwSettingsData currentHwSettings;
HwSettingsGet(&currentHwSettings);
@ -458,12 +458,12 @@ static void updateStats()
float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
const float STM32_TEMP_V25 = 0.76f; /* V */
const float STM32_TEMP_AVG_SLOPE = 2.5f; /* 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;
#else
float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
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 / STM32_TEMP_AVG_SLOPE + 25;
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
#endif
#endif
SystemStatsSet(&stats);
@ -552,7 +552,8 @@ void vApplicationIdleHook(void)
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
void vApplicationStackOverflowHook(__attribute__((unused))xTaskHandle * pxTask,
__attribute__((unused))signed portCHAR * pcTaskName)
{
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW

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

@ -304,7 +304,7 @@ static void processObjEvent(UAVObjEvent * ev)
/**
* Telemetry transmit task, regular priority
*/
static void telemetryTxTask(void *parameters)
static void telemetryTxTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
@ -322,7 +322,7 @@ static void telemetryTxTask(void *parameters)
* Telemetry transmit task, high priority
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
static void telemetryTxPriTask(void *parameters)
static void telemetryTxPriTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
@ -340,7 +340,7 @@ static void telemetryTxPriTask(void *parameters)
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
static void telemetryRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
@ -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;

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

@ -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;
}

24
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -159,7 +159,7 @@ static float throttleOffset = 0;
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
@ -279,8 +279,8 @@ static void vtolPathFollowerTask(void *parameters)
*/
static void updatePOIBearing()
{
const float DEADBAND_HIGH = 0.10;
const float DEADBAND_LOW = -0.10;
const float DEADBAND_HIGH = 0.10f;
const float DEADBAND_LOW = -0.10f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
@ -296,20 +296,20 @@ static void updatePOIBearing()
float dLoc[3];
float yaw = 0;
float elevation = 0;
/*float elevation = 0;*/
dLoc[0] = positionActual.North - poi.North;
dLoc[1] = positionActual.East - poi.East;
dLoc[2] = positionActual.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180;
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
@ -335,11 +335,11 @@ static void updatePOIBearing()
// don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) {
if (pathAngle != 0 || changeRadius != 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
@ -347,7 +347,7 @@ static void updatePOIBearing()
//not above
if (distance >= 3.0f) {
//You can feed this into camerastabilization
elevation = RAD2DEG(atan2f(dLoc[2],distance));
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
@ -732,7 +732,7 @@ static float bound(float val, float min, float max)
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
}

4
flight/pios/common/libraries/dosfs/dosfs.c
View File

@ -1194,7 +1194,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
// this point, all passes through the read loop will be aligned on a
// sector boundary, which allows us to go through the optimal path
// 2A below.
if (remain >= SECTOR_SIZE - tempsize) {
if (remain >= (uint32_t)SECTOR_SIZE - tempsize) {
memcpy(scratch + tempsize, buffer, SECTOR_SIZE - tempsize);
if (!result)
result = DFS_WriteSector(fileinfo->volinfo->unit, scratch, sector, 1);
@ -1330,7 +1330,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
No original function of DosFS driver
It has no effect if writing to SD Card, it's only used by the DosFS wrapper in emulation
*/
uint32_t DFS_Close(PFILEINFO fileinfo)
uint32_t DFS_Close(__attribute__((unused)) PFILEINFO fileinfo)
{
return DFS_OK;
}

2
flight/pios/common/libraries/msheap/msheap.c
View File

@ -423,7 +423,7 @@ msheap_extend(uint32_t size)
* @param reason The reason we are panicking.
*/
void
msheap_panic(const char *reason)
msheap_panic(__attribute__((unused)) const char *reason)
{
for (;;)
;

2
flight/pios/common/libraries/msheap/pios_msheap.c
View File

@ -134,7 +134,7 @@ free(void *p)
#endif /* PIOS_INCLUDE_FREERTOS */
void
msheap_panic(const char *reason)
msheap_panic(__attribute__((unused)) const char *reason)
{
//PIOS_DEBUG_Panic(reason);
}

2
flight/pios/common/pios_adxl345.c
View File

@ -187,7 +187,7 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth)
/**
* @brief Enable measuring. This also disables the activity sensors (tap or free fall)
*/
static int32_t PIOS_ADXL345_SetMeasure(uint8_t enable)
static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;

2
flight/pios/common/pios_bma180.c
View File

@ -438,7 +438,7 @@ bool PIOS_BMA180_IRQHandler(void)
{
bma180_irqs++;
const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t pios_bma180_dmabuf[8];
// If we can't get the bus then just move on for efficiency

4
flight/pios/common/pios_bmp085.c
View File

@ -311,8 +311,8 @@ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer)
int32_t PIOS_BMP085_Test()
{
// TODO: Is there a better way to test this than just checking that pressure/temperature has changed?
int32_t passed = 1;
int32_t cur_value = 0;
uint32_t passed = 1;
uint32_t cur_value = 0;
cur_value = Temperature;
PIOS_BMP085_StartADC(TemperatureConv);

4
flight/pios/common/pios_com_msg.c
View File

@ -76,7 +76,7 @@ int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driv
return(0);
}
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
{
struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;
@ -103,7 +103,7 @@ static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint
return (bytes_from_fifo);
}
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
{
struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;

4
flight/pios/common/pios_flash_jedec.c
View File

@ -303,12 +303,12 @@ static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
#else /* FLASH_USE_FREERTOS_LOCKS */
static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id)
static int32_t PIOS_Flash_Jedec_StartTransaction(__attribute__((unused)) uintptr_t flash_id)
{
return 0;
}
static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t flash_id)
{
return 0;
}

8
flight/pios/common/pios_flashfs_logfs.c
View File

@ -818,7 +818,7 @@ static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_
* @retval -5 if filesystem is full even after garbage collection should have freed space
* @retval -6 if writing the new object to the filesystem failed
*/
int32_t PIOS_FLASHFS_ObjSave(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
int32_t PIOS_FLASHFS_ObjSave(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
{
int8_t rc;
@ -896,7 +896,7 @@ out_exit:
* @retval -3 if object size in filesystem does not exactly match buffer size
* @retval -4 if reading the object data from flash fails
*/
int32_t PIOS_FLASHFS_ObjLoad(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
int32_t PIOS_FLASHFS_ObjLoad(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
{
int8_t rc;
@ -955,7 +955,7 @@ out_exit:
* @retval -1 if failed to start transaction
* @retval -2 if failed to delete the object from the filesystem
*/
int32_t PIOS_FLASHFS_ObjDelete(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
int32_t PIOS_FLASHFS_ObjDelete(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
{
int8_t rc;
@ -988,7 +988,7 @@ out_exit:
* @retval -3 if failed to activate arena 0
* @retval -4 if failed to mount arena 0
*/
int32_t PIOS_FLASHFS_Format(uint32_t fs_id)
int32_t PIOS_FLASHFS_Format(__attribute__((unused)) uint32_t fs_id)
{
int32_t rc;

4
flight/pios/common/pios_gcsrcvr.c
View File

@ -113,7 +113,7 @@ static void gcsreceiver_updated(UAVObjEvent * ev)
}
}
extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id)
{
struct pios_gcsrcvr_dev *gcsrcvr_dev;
@ -145,7 +145,7 @@ extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
* \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
* \output >=0 channel value
*/
static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel)
{
if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
/* channel is out of range */

4
flight/pios/common/pios_mpu6000.c
View File

@ -454,7 +454,7 @@ bool PIOS_MPU6000_IRQHandler(void)
return false;
mpu6000_count = PIOS_MPU6000_FifoDepth(true);
if (mpu6000_count < sizeof(struct pios_mpu6000_data))
if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data))
return false;
if (PIOS_MPU6000_ClaimBus(true) != 0)
@ -474,7 +474,7 @@ bool PIOS_MPU6000_IRQHandler(void)
static struct pios_mpu6000_data data;
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (sizeof(data) * 2)) {
if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;

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

@ -208,7 +208,7 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
/* The state transition table */
const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
// Initialization thread
[RADIO_STATE_UNINITIALIZED] = {
@ -1573,11 +1573,11 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
static void pios_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;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
@ -1658,11 +1658,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;
@ -2161,7 +2161,7 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
*
* @param[in] rfm22b_dev The device structure
*/
static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
{
#ifdef PIOS_PPM_RECEIVER
// Only send PPM if we're connected
@ -2610,7 +2610,7 @@ static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
* @parem [in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
{
// RF module error .. flash the LED's
rfm22_clearLEDs();

5
flight/pios/common/pios_rfm22b_com.c
View File

@ -91,7 +91,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud)
* @param[in] rfm22b_dev The device ID.
* @param[in] rx_bytes_available The number of bytes available to receive
*/
static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id,
__attribute__((unused)) uint16_t rx_bytes_avail)
{
}
@ -101,7 +102,7 @@ static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
* @param[in] rfm22b_dev The device ID.
* @param[in] tx_bytes_available The number of bytes available to transmit
*/
static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {

2
flight/pios/inc/pios_rfm22b_priv.h
View File

@ -822,7 +822,7 @@ struct pios_rfm22b_dev {
#ifdef PIOS_INCLUDE_RFM22B_RCVR
// The PPM channel values
uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ppm_supv_timer;
uint32_t ppm_supv_timer;
bool ppm_fresh;
#endif
};

6
flight/pios/pios_sim_posix.h
View File

@ -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>

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

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

13
flight/pios/stm32f10x/pios_debug.c
View File

@ -41,7 +41,8 @@ static uint8_t debug_num_channels;
/**
* Initialise Debug-features
*/
void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels,
__attribute__((unused)) uint8_t num_channels)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
PIOS_Assert(channels);
@ -75,7 +76,7 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
* Set debug-pin high
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinHigh(uint8_t pin)
void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -93,7 +94,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
* Set debug-pin low
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinLow(uint8_t pin)
void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -108,7 +109,7 @@ void PIOS_DEBUG_PinLow(uint8_t pin)
}
void PIOS_DEBUG_PinValue8Bit(uint8_t value)
void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -131,7 +132,7 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value)
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue4BitL(uint8_t value)
void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -151,7 +152,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
/**
* Report a serious error and halt
*/
void PIOS_DEBUG_Panic(const char *msg)
void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg)
{
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
register int *lr asm("lr"); // Link-register holds the PC of the caller

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

@ -88,7 +88,7 @@ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len)
// write 4 bytes at a time into program flash area (emulated EEPROM area)
uint8_t *p1 = data;
uint32_t *p3 = (uint32_t *)config.base_address;
for (int32_t i = 0; i < size; p3++)
for (uint32_t i = 0; i < size; p3++)
{
uint32_t value = 0;

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

@ -34,7 +34,7 @@
#include <pios_led_priv.h>
const static struct pios_led_cfg * led_cfg;
static const struct pios_led_cfg * led_cfg;
/**
* Initialises all the LED's

14
flight/pios/stm32f10x/pios_ppm.c
View File

@ -114,7 +114,7 @@ static struct pios_ppm_dev * PIOS_PPM_alloc(void)
static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PPM_tim_overflow_cb,
.edge = PIOS_PPM_tim_edge_cb,
};
@ -224,7 +224,10 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
return ppm_dev->CaptureValue[channel];
}
static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
__attribute__((unused)) uint8_t channel,
uint16_t count)
{
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -239,7 +242,10 @@ static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
}
static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
uint8_t chan_idx,
uint16_t count)
{
/* Recover our device context */
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -287,7 +293,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha
/* Check if the last frame was well formed */
if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
/* The last frame was well formed */
for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
for (int32_t i = 0; i < ppm_dev->NumChannels; i++) {
ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
}
for (uint32_t i = ppm_dev->NumChannels;

7
flight/pios/stm32f10x/pios_ppm_out.c
View File

@ -98,7 +98,7 @@ static struct pios_ppm_out_dev * PIOS_PPM_alloc(void)
#endif
static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
const static struct pios_tim_callbacks tim_out_callbacks = {
static const struct pios_tim_callbacks tim_out_callbacks = {
.overflow = NULL,
.edge = PIOS_PPM_OUT_tim_edge_cb,
};
@ -203,7 +203,10 @@ void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position)
ppm_dev->ChannelValue[servo] = position;
}
static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PPM_OUT_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
uint32_t context,
__attribute__((unused)) uint8_t chan_idx,
__attribute__((unused)) uint16_t count)
{
struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context;
if (!PIOS_PPM_Out_validate(ppm_dev))

8
flight/pios/stm32f10x/pios_pwm.c
View File

@ -44,7 +44,7 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
/* Local Variables */
/* 100 ms timeout without updates on channels */
const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
enum pios_pwm_dev_magic {
PIOS_PWM_DEV_MAGIC = 0xab30293c,
@ -98,7 +98,7 @@ static struct pios_pwm_dev * PIOS_PWM_alloc(void)
static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PWM_tim_overflow_cb,
.edge = PIOS_PWM_tim_edge_cb,
};
@ -193,7 +193,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
return pwm_dev->CaptureValue[channel];
}
static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
{
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
@ -219,7 +219,7 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
return;
}
static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;

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

@ -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()

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

@ -39,7 +39,7 @@
#include <pios_spi_priv.h>
static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev)
{
/* Should check device magic here */
return(true);

4
flight/pios/stm32f10x/pios_usart.c
View File

@ -196,7 +196,7 @@ out_fail:
return(-1);
}
static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
@ -205,7 +205,7 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
}
static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail)
static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;

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

@ -208,7 +208,7 @@ int32_t PIOS_USB_Reenumerate()
return 0;
}
bool PIOS_USB_CableConnected(uint8_t id)
bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id;

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

@ -229,7 +229,7 @@ static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev)
#endif /* PIOS_INCLUDE_FREERTOS */
}
static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail)
static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;

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

@ -223,7 +223,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) {
PIOS_IRQ_Enable();
}
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail)
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id;

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

@ -44,7 +44,7 @@ struct {
* Output : None.
* Return : None
*******************************************************************************/
void USB_Cable_Config(FunctionalState NewState)
void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState)
{
}

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

@ -52,7 +52,7 @@ void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size)
static ONE_DESCRIPTOR Config_Descriptor;
void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size)
void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size)
{
Config_Descriptor.Descriptor = desc;
Config_Descriptor.Descriptor_Size = desc_size;

2
flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_core.c
View File

@ -156,7 +156,7 @@ void USBD_Init(USB_OTG_CORE_HANDLE *pdev,
* @param pdev: device instance
* @retval status: status
*/
USBD_Status USBD_DeInit(USB_OTG_CORE_HANDLE *pdev)
USBD_Status USBD_DeInit(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev)
{
/* Software Init */

4
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h
View File

@ -53,10 +53,10 @@ typedef struct _USBD_DCD_INT
uint8_t (* Resume) (USB_OTG_CORE_HANDLE *pdev);
uint8_t (* IsoINIncomplete) (USB_OTG_CORE_HANDLE *pdev);
uint8_t (* IsoOUTIncomplete) (USB_OTG_CORE_HANDLE *pdev);
#ifdef VBUS_SENSING_ENABLED
uint8_t (* DevConnected) (USB_OTG_CORE_HANDLE *pdev);
uint8_t (* DevDisconnected) (USB_OTG_CORE_HANDLE *pdev);
#endif
}USBD_DCD_INT_cb_TypeDef;
extern USBD_DCD_INT_cb_TypeDef *USBD_DCD_INT_fops;

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)
{
/* 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);

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 */
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;

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)
{
/* 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);

12
flight/pios/stm32f4xx/pios_adc.c
View File

@ -130,7 +130,7 @@ init_pins(void)
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) {
if (config[i].port == NULL)
continue;
GPIO_InitStructure.GPIO_Pin = config[i].pin;
@ -210,7 +210,7 @@ init_adc(void)
ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE);
/* Configure input scan */
for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev,
config[i].channel,
i+1,
@ -284,7 +284,7 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg)
* @brief Configure the ADC to run at a fixed oversampling
* @param[in] oversampling the amount of oversampling to run at
*/
void PIOS_ADC_Config(uint32_t oversampling)
void PIOS_ADC_Config(__attribute__((unused)) uint32_t oversampling)
{
/* we ignore this */
}
@ -376,7 +376,7 @@ uint8_t PIOS_ADC_GetOverSampling(void)
* filter coefficients
* @note Not currently supported.
*/
void PIOS_ADC_SetFIRCoefficients(float * new_filter)
void PIOS_ADC_SetFIRCoefficients(__attribute__((unused)) float * new_filter)
{
// not implemented
}
@ -393,14 +393,14 @@ void accumulate(uint16_t *buffer, uint32_t count)
* Accumulate sampled values.
*/
while (count--) {
for (int i = 0; i < PIOS_ADC_NUM_PINS; i++) {
for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) {
accumulator[i].accumulator += *sp++;
accumulator[i].count++;
/*
* If the accumulator reaches half-full, rescale in order to
* make more space.
*/
if (accumulator[i].accumulator >= (1 << 31)) {
if (accumulator[i].accumulator >= (((uint32_t)1) << 31)) {
accumulator[i].accumulator /= 2;
accumulator[i].count /= 2;
}

12
flight/pios/stm32f4xx/pios_debug.c
View File

@ -41,7 +41,7 @@ static uint8_t debug_num_channels;
/**
* Initialise Debug-features
*/
void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels, __attribute__((unused)) uint8_t num_channels)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
PIOS_Assert(channels);
@ -77,7 +77,7 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
* Set debug-pin high
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinHigh(uint8_t pin)
void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -95,7 +95,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
* Set debug-pin low
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinLow(uint8_t pin)
void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
@ -110,7 +110,7 @@ void PIOS_DEBUG_PinLow(uint8_t pin)
}
void PIOS_DEBUG_PinValue8Bit(uint8_t value)
void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -136,7 +136,7 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value)
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue4BitL(uint8_t value)
void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
@ -159,7 +159,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
/**
* Report a serious error and halt
*/
void PIOS_DEBUG_Panic(const char *msg)
void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg)
{
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
register int *lr asm("lr"); // Link-register holds the PC of the caller

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

@ -166,7 +166,7 @@ static struct pios_internal_flash_dev * PIOS_Flash_Internal_alloc(void)
#endif /* defined(PIOS_INCLUDE_FREERTOS) */
int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, const struct pios_flash_internal_cfg * cfg)
int32_t PIOS_Flash_Internal_Init(uintptr_t * flash_id, __attribute__((unused)) const struct pios_flash_internal_cfg * cfg)
{
struct pios_internal_flash_dev * flash_dev;

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

@ -132,7 +132,7 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter);
static void i2c_adapter_log_fault(enum pios_i2c_error_type type);
static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter);
const static struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = {
static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = {
[I2C_STATE_FSM_FAULT] = {
.entry_fn = go_fsm_fault,
.next_state = {
@ -617,9 +617,6 @@ static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum
* guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was.
*/
enum i2c_adapter_state prev_state = i2c_adapter->curr_state;
if (prev_state) ;
i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[event];
/* Call the entry function (if any) for the next state. */
@ -637,9 +634,6 @@ static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_IRQ_Disable();
enum i2c_adapter_state prev_state = i2c_adapter->curr_state;
if (prev_state) ;
while (i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO]) {
i2c_adapter->curr_state = i2c_adapter_transitions[i2c_adapter->curr_state].next_state[I2C_EVENT_AUTO];
@ -668,7 +662,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) {

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

@ -34,7 +34,7 @@
#include <pios_led_priv.h>
const static struct pios_led_cfg * led_cfg;
static const struct pios_led_cfg * led_cfg;
/**
* Initialises all the LED's

4
flight/pios/stm32f4xx/pios_overo.c
View File

@ -308,7 +308,7 @@ out_fail:
return(-1);
}
static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail)
static void PIOS_OVERO_RxStart(uint32_t overo_id, __attribute__((unused)) uint16_t rx_bytes_avail)
{
struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id;
@ -318,7 +318,7 @@ static void PIOS_OVERO_RxStart(uint32_t overo_id, uint16_t rx_bytes_avail)
// DMA RX enable (enable IRQ) ?
}
static void PIOS_OVERO_TxStart(uint32_t overo_id, uint16_t tx_bytes_avail)
static void PIOS_OVERO_TxStart(uint32_t overo_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_overo_dev * overo_dev = (struct pios_overo_dev *)overo_id;

9
flight/pios/stm32f4xx/pios_ppm.c
View File

@ -113,7 +113,7 @@ static struct pios_ppm_dev * PIOS_PPM_alloc(void)
static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PPM_tim_overflow_cb,
.edge = PIOS_PPM_tim_edge_cb,
};
@ -222,7 +222,8 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
return ppm_dev->CaptureValue[channel];
}
static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context,
__attribute__((unused)) uint8_t channel, uint16_t count)
{
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -237,7 +238,7 @@ static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
}
static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@ -285,7 +286,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha
/* Check if the last frame was well formed */
if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
/* The last frame was well formed */
for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
for (int32_t i = 0; i < ppm_dev->NumChannels; i++) {
ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
}
for (uint32_t i = ppm_dev->NumChannels;

8
flight/pios/stm32f4xx/pios_pwm.c
View File

@ -43,7 +43,7 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
/* Local Variables */
/* 100 ms timeout without updates on channels */
const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
enum pios_pwm_dev_magic {
PIOS_PWM_DEV_MAGIC = 0xab30293c,
@ -97,7 +97,7 @@ static struct pios_pwm_dev * PIOS_PWM_alloc(void)
static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
static const struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PWM_tim_overflow_cb,
.edge = PIOS_PWM_tim_edge_cb,
};
@ -192,7 +192,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
return pwm_dev->CaptureValue[channel];
}
static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
{
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
@ -218,7 +218,7 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
return;
}
static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;

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

@ -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()

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

@ -44,7 +44,7 @@
#define SPI_MAX_BLOCK_PIO 128
static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev)
{
/* Should check device magic here */
return(true);

4
flight/pios/stm32f4xx/pios_usart.c
View File

@ -227,7 +227,7 @@ out_fail:
return(-1);
}
static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
@ -236,7 +236,7 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
}
static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail)
static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;

6
flight/pios/stm32f4xx/pios_usb.c
View File

@ -155,7 +155,7 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected)
* \return 0: interface not available
*/
uint32_t usb_found;
bool PIOS_USB_CheckAvailable(uint32_t id)
bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id;
@ -175,7 +175,7 @@ bool PIOS_USB_CheckAvailable(uint32_t id)
#include "usb_bsp.h"
void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
void USB_OTG_BSP_Init(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id;
@ -210,7 +210,7 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE);
}
void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev)
void USB_OTG_BSP_EnableInterrupt(__attribute__((unused)) USB_OTG_CORE_HANDLE *pdev)
{
struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id;

8
flight/pios/stm32f4xx/pios_usb_hid.c
View File

@ -261,7 +261,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) {
}
}
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail)
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail)
{
struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id;
@ -428,7 +428,7 @@ static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request
return true;
}
static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_request *req)
static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid_id, __attribute__((unused)) struct usb_setup_request *req)
{
/* HID devices don't have any OUT data stages on the control endpoint */
PIOS_Assert(0);
@ -438,7 +438,7 @@ static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_re
* @brief Callback used to indicate a transmission from device INto host completed
* Checks if any data remains, pads it into HID packet and sends.
*/
static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len)
static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, __attribute__((unused)) uint16_t len)
{
struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id;
@ -460,7 +460,7 @@ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint
/**
* EP1 OUT Callback Routine
*/
static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len)
static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, uint16_t len)
{
struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id;

36
flight/pios/stm32f4xx/pios_usbhook.c
View File

@ -69,7 +69,7 @@ void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t *
static struct pios_usbhook_descriptor Config_Descriptor;
void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size)
void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size)
{
Config_Descriptor.descriptor = desc;
Config_Descriptor.length = desc_size;
@ -205,42 +205,42 @@ void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len)
* Device level hooks into STM USB library
*/
static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = Device_Descriptor.length;
return Device_Descriptor.descriptor;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = String_Descriptor[USB_STRING_DESC_LANG].length;
return String_Descriptor[USB_STRING_DESC_LANG].descriptor;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = String_Descriptor[USB_STRING_DESC_VENDOR].length;
return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = String_Descriptor[USB_STRING_DESC_PRODUCT].length;
return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = String_Descriptor[USB_STRING_DESC_SERIAL].length;
return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length)
{
return NULL;
}
static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length)
{
return NULL;
}
@ -266,7 +266,7 @@ static void PIOS_USBHOOK_USR_Init(void)
#endif
}
static void PIOS_USBHOOK_USR_DeviceReset(uint8_t speed)
static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed)
{
PIOS_USB_ChangeConnectionState(false);
}
@ -306,7 +306,7 @@ static USBD_Usr_cb_TypeDef user_callbacks = {
.DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected,
};
static uint8_t PIOS_USBHOOK_CLASS_Init(void *pdev, uint8_t cfgidx)
static uint8_t PIOS_USBHOOK_CLASS_Init(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx)
{
/* Call all of the registered init callbacks */
for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) {
@ -318,7 +318,7 @@ static uint8_t PIOS_USBHOOK_CLASS_Init(void *pdev, uint8_t cfgidx)
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_DeInit(void *pdev, uint8_t cfgidx)
static uint8_t PIOS_USBHOOK_CLASS_DeInit(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx)
{
/* Call all of the registered deinit callbacks */
for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) {
@ -331,7 +331,7 @@ static uint8_t PIOS_USBHOOK_CLASS_DeInit(void *pdev, uint8_t cfgidx)
}
static struct usb_setup_request usb_ep0_active_req;
static uint8_t PIOS_USBHOOK_CLASS_Setup(void *pdev, USB_SETUP_REQ *req)
static uint8_t PIOS_USBHOOK_CLASS_Setup(__attribute__((unused)) void *pdev, USB_SETUP_REQ *req)
{
switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) {
case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE):
@ -365,12 +365,12 @@ static uint8_t PIOS_USBHOOK_CLASS_Setup(void *pdev, USB_SETUP_REQ *req)
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(void *pdev)
static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(__attribute__((unused)) void *pdev)
{
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(void *pdev)
static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(__attribute__((unused)) void *pdev)
{
uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex);
@ -418,22 +418,22 @@ static uint8_t PIOS_USBHOOK_CLASS_DataOut(void *pdev, uint8_t epnum)
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_SOF(void *pdev)
static uint8_t PIOS_USBHOOK_CLASS_SOF(__attribute__((unused)) void *pdev)
{
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(void *pdev)
static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(__attribute__((unused)) void *pdev)
{
return USBD_OK;
}
static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(void *pdev)
static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(__attribute__((unused)) void *pdev)
{
return USBD_OK;
}
static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(uint8_t speed, uint16_t *length)
static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length)
{
*length = Config_Descriptor.length;
return Config_Descriptor.descriptor;

Some files were not shown because too many files have changed in this diff Show More