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

Merge remote-tracking branch 'origin/next' into thread/OP-935_Changes_Basic_Stab

Browse Source
This commit is contained in:
Fredrik Arvidsson 2013-05-06 22:48:57 +02:00
commit 23bc774a03
136 changed files with 2532 additions and 1975 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);

96
flight/libraries/inc/packet_handler.h
View File

@ -1,16 +1,16 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@ -41,66 +41,66 @@
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_CON_REQUEST, // request a connection to another modem
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK, // Acknowlege the receipt of a packet
PACKET_TYPE_ACK_RTS, // Acknowlege the receipt of a packet and indicate that the receiving side has data to send (ready to send)
PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet
PACKET_TYPE_NONE = 0,
PACKET_TYPE_STATUS, // broadcasts status of this modem
PACKET_TYPE_CON_REQUEST, // request a connection to another modem
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK, // Acknowlege the receipt of a packet
PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet
} PHPacketType;
typedef struct {
uint32_t destination_id;
uint16_t seq_num;
uint8_t type;
uint8_t data_size;
uint32_t destination_id;
portTickType prev_tx_time;
uint16_t seq_num;
uint8_t type;
uint8_t data_size;
} PHPacketHeader;
#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY)
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
PHPacketHeader header;
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle;
#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
portTickType packet_recv_time;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint8_t ecc[RS_ECC_NPARITY];
} PHAckNackPacket, *PHAckNackPacketHandle;
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
#define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint32_t source_id;
uint8_t link_quality;
int8_t received_rssi;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint32_t source_id;
uint8_t link_quality;
int8_t received_rssi;
uint8_t ecc[RS_ECC_NPARITY];
} PHStatusPacket, *PHStatusPacketHandle;
#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct {
PHPacketHeader header;
uint32_t source_id;
uint32_t min_frequency;
uint32_t max_frequency;
uint32_t channel_spacing;
OPLinkSettingsMainPortOptions main_port;
OPLinkSettingsFlexiPortOptions flexi_port;
OPLinkSettingsVCPPortOptions vcp_port;
OPLinkSettingsComSpeedOptions com_speed;
uint8_t ecc[RS_ECC_NPARITY];
PHPacketHeader header;
uint32_t source_id;
uint32_t min_frequency;
uint32_t max_frequency;
uint32_t channel_spacing;
portTickType status_rx_time;
OPLinkSettingsMainPortOptions main_port;
OPLinkSettingsFlexiPortOptions flexi_port;
OPLinkSettingsVCPPortOptions vcp_port;
OPLinkSettingsComSpeedOptions com_speed;
uint8_t ecc[RS_ECC_NPARITY];
} PHConnectionPacket, *PHConnectionPacketHandle;
#endif // __PACKET_HANDLER_H__

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;

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

@ -141,37 +141,6 @@ static int32_t RadioComBridgeStart(void)
bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id);
if (is_coordinator) {
// Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) {
case OPLINKSETTINGS_MAXRFPOWER_125:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
break;
case OPLINKSETTINGS_MAXRFPOWER_16:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
break;
case OPLINKSETTINGS_MAXRFPOWER_316:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
break;
case OPLINKSETTINGS_MAXRFPOWER_63:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
break;
case OPLINKSETTINGS_MAXRFPOWER_126:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
break;
case OPLINKSETTINGS_MAXRFPOWER_25:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
break;
case OPLINKSETTINGS_MAXRFPOWER_50:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
break;
case OPLINKSETTINGS_MAXRFPOWER_100:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
break;
default:
// do nothing
break;
}
// Set the frequency range.
PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing);
@ -184,6 +153,37 @@ static int32_t RadioComBridgeStart(void)
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
}
// Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) {
case OPLINKSETTINGS_MAXRFPOWER_125:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
break;
case OPLINKSETTINGS_MAXRFPOWER_16:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
break;
case OPLINKSETTINGS_MAXRFPOWER_316:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
break;
case OPLINKSETTINGS_MAXRFPOWER_63:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
break;
case OPLINKSETTINGS_MAXRFPOWER_126:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
break;
case OPLINKSETTINGS_MAXRFPOWER_25:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
break;
case OPLINKSETTINGS_MAXRFPOWER_50:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
break;
case OPLINKSETTINGS_MAXRFPOWER_100:
PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
break;
default:
// do nothing
break;
}
// Set the initial frequency.
PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency);
@ -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;

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

@ -71,7 +71,6 @@
#define RFM22B_MAXIMUM_FREQUENCY 440000000
#define RFM22B_DEFAULT_FREQUENCY 433000000
#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000
//#define RFM22B_TEST_DROPPED_PACKETS 1
// The maximum amount of time since the last message received to consider the connection broken.
#define DISCONNECT_TIMEOUT_MS 1000 // ms
@ -125,8 +124,8 @@
/* Local type definitions */
struct pios_rfm22b_transition {
enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
enum pios_rfm22b_state next_state[RFM22B_EVENT_NUM_EVENTS];
enum pios_radio_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS];
};
// Must ensure these prefilled arrays match the define sizes
@ -157,42 +156,39 @@ static const uint8_t OUT_FF[64] = {
/* Local function forwared declarations */
static void pios_rfm22_task(void *parameters);
static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR);
static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR);
static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status);
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size);
static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel);
static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev);
static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks);
static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev);
static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_clearLEDs();
@ -206,220 +202,187 @@ static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
/* The state transition table */
const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = {
static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
// Initialization thread
[RFM22B_STATE_UNINITIALIZED] = {
[RADIO_STATE_UNINITIALIZED] = {
.entry_fn = 0,
.next_state = {
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
},
},
[RFM22B_STATE_INITIALIZING] = {
[RADIO_STATE_INITIALIZING] = {
.entry_fn = rfm22_init,
.next_state = {
[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_REQUESTING_CONNECTION] = {
[RADIO_STATE_REQUESTING_CONNECTION] = {
.entry_fn = rfm22_requestConnection,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR
},
},
[RFM22B_STATE_ACCEPTING_CONNECTION] = {
[RADIO_STATE_ACCEPTING_CONNECTION] = {
.entry_fn = rfm22_acceptConnection,
.next_state = {
[RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_MODE] = {
.entry_fn = rfm22_setRxMode,
[RADIO_STATE_RX_MODE] = {
.entry_fn = radio_setRxMode,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
[RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_WAIT_PREAMBLE] = {
.entry_fn = rfm22_detectPreamble,
[RADIO_STATE_RX_DATA] = {
.entry_fn = radio_rxData,
.next_state = {
[RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC,
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION,
[RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS,
[RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION,
[RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK,
[RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_WAIT_SYNC] = {
.entry_fn = rfm22_detectSync,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC,
[RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_DATA] = {
.entry_fn = rfm22_rxData,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA,
[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK,
[RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS,
[RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION,
[RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK,
[RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RX_FAILURE] = {
.entry_fn = rfm22_rxFailure,
.next_state = {
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_ACK] = {
[RADIO_STATE_RECEIVING_ACK] = {
.entry_fn = rfm22_receiveAck,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_NACK] = {
[RADIO_STATE_RECEIVING_NACK] = {
.entry_fn = rfm22_receiveNack,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_RECEIVING_STATUS] = {
[RADIO_STATE_RECEIVING_STATUS] = {
.entry_fn = rfm22_receiveStatus,
.next_state = {
[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_START] = {
.entry_fn = rfm22_txStart,
[RADIO_STATE_TX_START] = {
.entry_fn = radio_txStart,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_DATA] = {
.entry_fn = rfm22_txData,
[RADIO_STATE_TX_DATA] = {
.entry_fn = radio_txData,
.next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
[RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION,
[RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
[RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TX_FAILURE] = {
[RADIO_STATE_TX_FAILURE] = {
.entry_fn = rfm22_txFailure,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_SENDING_ACK] = {
[RADIO_STATE_SENDING_ACK] = {
.entry_fn = rfm22_sendAck,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_SENDING_NACK] = {
[RADIO_STATE_SENDING_NACK] = {
.entry_fn = rfm22_sendNack,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_TIMEOUT] = {
[RADIO_STATE_TIMEOUT] = {
.entry_fn = rfm22_timeout,
.next_state = {
[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_ERROR] = {
[RADIO_STATE_ERROR] = {
.entry_fn = rfm22_error,
.next_state = {
[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RFM22B_STATE_FATAL_ERROR] = {
[RADIO_STATE_FATAL_ERROR] = {
.entry_fn = rfm22_fatal_error,
.next_state = {
},
@ -511,6 +474,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->stats.rssi = 0;
rfm22b_dev->stats.tx_seq = 0;
rfm22b_dev->stats.rx_seq = 0;
rfm22b_dev->stats.tx_failure = 0;
// Initialize the frequencies.
PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY);
@ -523,7 +487,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->coordinator = false;
// Create the event queue
rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event));
rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
// Bind the configuration to the device instance
rfm22b_dev->cfg = *cfg;
@ -560,10 +524,10 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
initialize_ecc();
// Set the state to initializing.
rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED;
rfm22b_dev->state = RADIO_STATE_UNINITIALIZED;
// Initialize the radio device.
pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
// Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler.
xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle));
@ -580,7 +544,7 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (PIOS_RFM22B_Validate(rfm22b_dev)) {
pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
}
}
@ -594,7 +558,7 @@ bool PIOS_RFM22_EXT_Int(void)
}
// Inject an interrupt event into the state machine.
pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true);
pios_rfm22_inject_event(g_rfm22b_dev, RADIO_EVENT_INT_RECEIVED, true);
return false;
}
@ -628,6 +592,21 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id)
return false;
}
/**
* Returns true if the modem is not actively sending or receiving a packet.
*
* @param[in] rfm22b_id The RFM22B device index.
* @return True if the modem is not actively sending or receiving a packet.
*/
bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id)
{
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (PIOS_RFM22B_Validate(rfm22b_dev)) {
return ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION));
}
return false;
}
/**
* Sets the radio device transmit power.
*
@ -790,6 +769,350 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id)
return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD));
}
/**
* Put the RFM22B device into receive mode.
*
* @param[in] rfm22b_id The rfm22b device.
* @param[in] p The packet to receive into.
* @return true if Rx mode was entered sucessfully.
*/
bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return false;
}
// Are we already in Rx mode?
if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT)) {
return false;
}
rfm22b_dev->rx_packet_handle = p;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// Switch to TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_OFF;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
RX_LED_OFF;
TX_LED_OFF;
// empty the rx buffer
rfm22b_dev->rx_buffer_wr = 0;
// Clear the TX buffer.
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// enable RX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
RFM22_ie2_enswdet);
// enable the receiver
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX wait mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT;
return true;
}
/**
* Transmit a packet via the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @param[in] p The packet to transmit.
* @return true if there if the packet was queued for transmission.
*/
bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return false;
}
rfm22b_dev->tx_packet = p;
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0) {
rfm22b_dev->packet_start_ticks = 1;
}
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// set the tx power
rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
// TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
// Queue the data up for sending
rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
RX_LED_OFF;
// Set the destination address in the transmit header.
// The destination address is the first 4 bytes of the message.
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk);
// Clear the FIFOs.
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// Set the total number of data bytes we are going to transmit.
rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
// Add some data to the chips TX FIFO before enabling the transmitter
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
// Enable TX interrupts.
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// Enable the transmitter.
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// We're in Tx mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_TX_MODE;
TX_LED_ON;
return true;
}
/**
* Process a Tx interrupt from the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
*/
pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
// Read the device status registers
if (!pios_rfm22_readStatus(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
// TX FIFO almost empty, it needs filling up
if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) {
// Add data to the TX FIFO buffer
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
return PIOS_RFM22B_INT_SUCCESS;
} else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) {
// Transition out of Tx mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
return PIOS_RFM22B_TX_COMPLETE;
}
return 0;
}
/**
* Process a Rx interrupt from the RFM22B device.
*
* @param[in] rfm22b_id The rfm22b device.
* @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
*/
pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) {
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
return PIOS_RFM22B_INT_FAILURE;
}
uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle);
pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS;
// Read the device status registers
if (!pios_rfm22_readStatus(rfm22b_dev)) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// FIFO under/over flow error. Restart RX mode.
if (rfm22b_dev->status_regs.int_status_1.fifo_underoverflow_error ||
rfm22b_dev->status_regs.int_status_1.crc_error) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Valid packet received
if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
}
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Is there a length error?
if (rfm22b_dev->rx_buffer_wr != len) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Increment the total byte received count.
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
// We're finished with Rx mode
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
ret = PIOS_RFM22B_RX_COMPLETE;
} else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
// RX FIFO almost full, it needs emptying
// read data from the rf chips FIFO buffer
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
rfm22_releaseBus(rfm22b_dev);
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs(rfm22b_dev);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Make sure that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Valid preamble detected
RX_LED_ON;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// We detected the preamble, now wait for sync.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
} else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
// Sync word detected
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Waiting for the preamble timed out.
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Set the packet start time if necessary.
if ((rfm22b_dev->packet_start_ticks == 0) &&
((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) {
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
}
return ret;
}
/**
* Validate that the device structure is valid.
*
@ -831,10 +1154,10 @@ static void pios_rfm22_task(void *parameters)
lastEventTicks = xTaskGetTickCount();
// Process events through the state machine.
enum pios_rfm22b_event event;
enum pios_radio_event event;
while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
if ((event == RFM22B_EVENT_INT_RECEIVED) &&
((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING)))
if ((event == RADIO_EVENT_INT_RECEIVED) &&
((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING)))
continue;
rfm22_process_event(rfm22b_dev, event);
}
@ -843,10 +1166,10 @@ static void pios_rfm22_task(void *parameters)
portTickType curTicks = xTaskGetTickCount();
if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) {
// Transsition through an error event.
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
// Clear the event queue.
enum pios_rfm22b_event event;
enum pios_radio_event event;
while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
// Do nothing;
}
@ -855,28 +1178,29 @@ static void pios_rfm22_task(void *parameters)
}
// Change channels if necessary.
if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) {
if (PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22_changeChannel(rfm22b_dev);
}
portTickType curTicks = xTaskGetTickCount();
uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks);
// Have we been sending this packet too long?
if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT);
// Has it been too long since we received a packet
// Have we been sending / receiving this packet too long?
if ((rfm22b_dev->packet_start_ticks > 0) &&
(pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT);
} else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
// Has it been too long since we received a packet
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
} else {
// Are we waiting for an ACK?
if (rfm22b_dev->prev_tx_packet) {
// Should we resend the packet?
if (pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) {
if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) &&
PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22b_dev->tx_complete_ticks = curTicks;
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT);
rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT);
}
} else {
@ -888,7 +1212,7 @@ static void pios_rfm22_task(void *parameters)
}
// Queue up a status packet if it's time.
if ((pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) {
if (pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) {
rfm22_sendStatus(rfm22b_dev);
lastStatusTicks = curTicks;
}
@ -897,21 +1221,16 @@ static void pios_rfm22_task(void *parameters)
}
// Send a packet if it's our time slice
rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0);
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
if (rfm22b_dev->time_to_send) {
bool time_to_send = rfm22_timeToSend(rfm22b_dev);
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
if (time_to_send) {
D4_LED_ON;
} else {
D4_LED_OFF;
}
if (rfm22_inChannelBuffer(rfm22b_dev)) {
D3_LED_ON;
} else {
D3_LED_OFF;
}
#endif
if (rfm22b_dev->time_to_send) {
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START);
if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START);
}
}
}
@ -928,22 +1247,25 @@ static void pios_rfm22_task(void *parameters)
* @param[in] event The event to inject
* @param[in] inISR Is this being called from an interrrup service routine?
*/
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR)
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR)
{
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (inISR) {
portBASE_TYPE pxHigherPriorityTaskWoken;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) {
// Store the event.
portBASE_TYPE pxHigherPriorityTaskWoken1;
if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
portBASE_TYPE pxHigherPriorityTaskWoken2;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
}
portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken);
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
} else {
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
@ -956,20 +1278,20 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
*
* @param[in] rfm22b_dev The device structure
* @param[in] event The event to process
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
{
// No event
if (event == RFM22B_EVENT_NUM_EVENTS) {
return RFM22B_EVENT_NUM_EVENTS;
if (event >= RADIO_EVENT_NUM_EVENTS) {
return RADIO_EVENT_NUM_EVENTS;
}
// Don't transition if there is no transition defined
enum pios_rfm22b_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
enum pios_radio_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
if (!next_state) {
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/*
@ -986,7 +1308,7 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev);
}
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/**
@ -996,10 +1318,10 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
* @param[in] rfm22b_dev The device structure
* @param[in] event The event to process
*/
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
{
// Process all state transitions.
while(event != RFM22B_EVENT_NUM_EVENTS) {
while(event != RADIO_EVENT_NUM_EVENTS) {
event = rfm22_process_state_transition(rfm22b_dev, event);
}
}
@ -1013,16 +1335,16 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf
* Initialize (or re-initialize) the RFM22B radio device.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
{
// Initialize the register values.
rfm22b_dev->device_status = 0;
rfm22b_dev->int_status1 = 0;
rfm22b_dev->int_status2 = 0;
rfm22b_dev->ezmac_status = 0;
rfm22b_dev->status_regs.int_status_1.raw = 0;
rfm22b_dev->status_regs.int_status_2.raw = 0;
rfm22b_dev->status_regs.device_status.raw = 0;
rfm22b_dev->status_regs.ezmac_status.raw = 0;
// Clean the LEDs
rfm22_clearLEDs();
@ -1043,8 +1365,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// Initialize the state
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
rfm22b_dev->destination_id = 0xffffffff;
rfm22b_dev->time_to_send = false;
rfm22b_dev->time_to_send_offset = 0;
rfm22b_dev->send_status = false;
rfm22b_dev->send_connection_request = false;
@ -1053,10 +1373,8 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->tx_packet = NULL;
rfm22b_dev->prev_tx_packet = NULL;
rfm22b_dev->data_packet.header.data_size = 0;
rfm22b_dev->in_rx_mode = false;
// Initialize the devide state
rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
rfm22b_dev->frequency_hop_channel = 0;
@ -1064,27 +1382,31 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_complete_ticks = 0;
rfm22b_dev->rx_complete_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING;
// software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
for (uint8_t i = 0; i < 50; ++i) {
for (int i = 50; i > 0; i--) {
// read the status registers
rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break;
pios_rfm22_readStatus(rfm22b_dev);
// wait 1ms
// Is the chip ready?
if (rfm22b_dev->status_regs.int_status_2.chip_ready)
break;
// Wait 1ms if not.
PIOS_DELAY_WaitmS(1);
}
// ****************
// read status - clears interrupt
rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status);
rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
pios_rfm22_readStatus(rfm22b_dev);
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// disable all interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
@ -1108,14 +1430,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
#endif
// incorrect RF module type
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
if (device_version != RFM22_DEVICE_VERSION_B1) {
#if defined(RFM22_DEBUG)
DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r");
#endif
// incorrect RF module version
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
// calibrate our RF module to be exactly on frequency .. different for every module
@ -1182,14 +1504,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// header control - using a 4 by header with broadcast of 0xffffffff
rfm22_write(rfm22b_dev, RFM22_header_control1,
RFM22_header_cntl1_bcen_0 |
RFM22_header_cntl1_bcen_1 |
RFM22_header_cntl1_bcen_2 |
RFM22_header_cntl1_bcen_3 |
RFM22_header_cntl1_hdch_0 |
RFM22_header_cntl1_hdch_1 |
RFM22_header_cntl1_hdch_2 |
RFM22_header_cntl1_hdch_3);
RFM22_header_cntl1_bcen_0 |
RFM22_header_cntl1_bcen_1 |
RFM22_header_cntl1_bcen_2 |
RFM22_header_cntl1_bcen_3 |
RFM22_header_cntl1_hdch_0 |
RFM22_header_cntl1_hdch_1 |
RFM22_header_cntl1_hdch_2 |
RFM22_header_cntl1_hdch_3);
// Check all bit of all bytes of the header
rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff);
rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff);
@ -1203,9 +1525,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff);
// 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
rfm22_write(rfm22b_dev, RFM22_header_control2,
RFM22_header_cntl2_hdlen_3210 |
RFM22_header_cntl2_synclen_3210 |
((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
RFM22_header_cntl2_hdlen_3210 |
RFM22_header_cntl2_synclen_3210 |
((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
// sync word
rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1);
@ -1213,9 +1535,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3);
rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4);
// set the tx power
rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
// TX FIFO Almost Full Threshold (0 - 63)
rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK);
@ -1228,11 +1547,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// Set the frequency calibration
rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// Initialize the frequency and datarate to te default.
rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE);
rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
pios_rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
return RFM22B_EVENT_INITIALIZED;
return RADIO_EVENT_INITIALIZED;
}
/**
@ -1248,14 +1570,17 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
* @param[in] datarate The air datarate.
* @param[in] data_whitening Is data whitening desired?
*/
static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
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);
// rfm22_if_filter_bandwidth
rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
@ -1309,6 +1634,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_da
rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00);
rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
}
/**
@ -1330,14 +1658,17 @@ 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;
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Calculate the number of frequency hopping channels.
rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size);
@ -1363,6 +1694,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff);
rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch);
rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
}
@ -1377,22 +1711,20 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t
if (rfm22b_dev->frequency_hop_channel == channel) {
return false;
}
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D3_LED_TOGGLE;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
rfm22b_dev->frequency_hop_channel = channel;
rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
return true;
}
/*****************************************************************************
* Radio Transmit and Receive functions.
*****************************************************************************/
/**
* Read the RFM22B interrupt and device status registers
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
// 1. Read the interrupt statuses with burst read
@ -1402,327 +1734,217 @@ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL);
rfm22_deassertCs(rfm22b_dev);
rfm22b_dev->int_status1 = read_buf[1];
rfm22b_dev->int_status2 = read_buf[2];
rfm22b_dev->status_regs.int_status_1.raw = read_buf[1];
rfm22b_dev->status_regs.int_status_2.raw = read_buf[2];
// Device status
rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status);
rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status);
// EzMAC status
rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status);
rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// the RF module has gone and done a reset - we need to re-initialize the rf module
if (rfm22b_dev->int_status2 & RFM22_is2_ipor) {
if (rfm22b_dev->status_regs.int_status_2.poweron_reset) {
return false;
}
return true;
}
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
}
/*****************************************************************************
* Radio Transmit and Receive functions.
*****************************************************************************/
/**
* Start a transmit if possible
*
* @param[in] radio_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
{
PHPacketHandle p = NULL;
// Don't send if it's not our turn, or if we're receiving a packet.
if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) {
return RADIO_EVENT_RX_MODE;
}
// See if there's a packet ready to send.
if (radio_dev->tx_packet) {
p = radio_dev->tx_packet;
} else {
// Don't send a packet if we're waiting for an ACK
if (radio_dev->prev_tx_packet) {
return RADIO_EVENT_RX_MODE;
}
// Send a connection request?
if (!p && radio_dev->send_connection_request) {
p = (PHPacketHandle)&(radio_dev->con_packet);
radio_dev->send_connection_request = false;
}
#ifdef PIOS_PPM_RECEIVER
// Send a PPM packet?
if (!p && radio_dev->send_ppm) {
p = (PHPacketHandle)&(radio_dev->ppm_packet);
radio_dev->send_ppm = false;
}
#endif
// Send status?
if (!p && radio_dev->send_status) {
p = (PHPacketHandle)&(radio_dev->status_packet);
radio_dev->send_status = false;
}
// Try to get some data to send
if (!p) {
bool need_yield = false;
p = &radio_dev->data_packet;
p->header.type = PACKET_TYPE_DATA;
p->header.destination_id = radio_dev->destination_id;
if (radio_dev->tx_out_cb && (p->header.data_size == 0)) {
p->header.data_size = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
}
// Don't send any data until we're connected.
if (!rfm22_isConnected(radio_dev)) {
p->header.data_size = 0;
}
if (p->header.data_size == 0) {
p = NULL;
}
}
}
if (!p) {
return RADIO_EVENT_RX_MODE;
}
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_ON;
#endif
// Add the packet sequence number.
p->header.seq_num = radio_dev->stats.tx_seq++;
// Pass the time of the previous transmitted packet to use for synchronizing the clocks.
p->header.prev_tx_time = radio_dev->tx_complete_ticks;
// Change the channel if necessary, but not when ACKing the connection request message.
if ((p->header.type != PACKET_TYPE_ACK) || (radio_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
rfm22_changeChannel(radio_dev);
}
// Add the error correcting code.
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
// Transmit the packet.
PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p);
return RADIO_EVENT_NUM_EVENTS;
}
/**
* Receive packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev)
{
enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
pios_rfm22b_int_result res = PIOS_RFM22B_ProcessTx((uint32_t)radio_dev);
// Is the transmition complete
if (res == PIOS_RFM22B_TX_COMPLETE) {
radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet);
radio_dev->tx_complete_ticks = xTaskGetTickCount();
// Is this an ACK?
bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK);
ret_event = RADIO_EVENT_RX_MODE;
if (is_ack) {
// If this is an ACK for a connection request message we need to
// configure this modem from the connection request message.
if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22_setConnectionParameters(radio_dev);
}
} else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) &&
(radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM)) {
// We need to wait for an ACK if this packet it not an ACK, NACK, or PPM.
radio_dev->prev_tx_packet = radio_dev->tx_packet;
}
radio_dev->tx_packet = 0;
radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0;
// Start a new transaction
radio_dev->packet_start_ticks = 0;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
#endif
}
return ret_event;
}
/**
* Switch the radio into receive mode.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
{
// Are we already in Rx mode?
if (rfm22b_dev->in_rx_mode) {
return RFM22B_EVENT_NUM_EVENTS;
if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, &(rfm22b_dev->rx_packet))) {
return RADIO_EVENT_NUM_EVENTS;
}
rfm22b_dev->packet_start_ticks = 0;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// Switch to TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
RX_LED_OFF;
TX_LED_OFF;
// empty the rx buffer
rfm22b_dev->rx_buffer_wr = 0;
// Clear the TX buffer.
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// enable RX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
RFM22_ie2_enswdet);
// enable the receiver
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
// Indicate that we're in RX mode.
rfm22b_dev->in_rx_mode = true;
// No event generated
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Detect the preamble
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev)
{
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev))
return RFM22B_EVENT_FAILURE;
// Valid preamble detected
if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) {
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
RX_LED_ON;
return RFM22B_EVENT_PREAMBLE_DETECTED;
}
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Detect the sync
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev)
{
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev))
return RFM22B_EVENT_FAILURE;
// Sync word detected
if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) {
RX_LED_ON;
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
return RFM22B_EVENT_SYNC_DETECTED;
} else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) {
// Waiting for sync timed out.
return RFM22B_EVENT_FAILURE;
}
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Receive the packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
{
// Swap in the next packet buffer if required.
uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet);
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev)) {
return RFM22B_EVENT_FAILURE;
}
// FIFO under/over flow error. Restart RX mode.
if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) {
return RFM22B_EVENT_FAILURE;
}
// RX FIFO almost full, it needs emptying
if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) {
// read data from the rf chips FIFO buffer
// read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
return RFM22B_EVENT_FAILURE;
}
// Another packet length error.
if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) {
return RFM22B_EVENT_FAILURE;
}
// Fetch the data from the RX FIFO
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
}
// CRC error .. discard the received data
if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) {
return RFM22B_EVENT_FAILURE;
}
// Valid packet received
if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) {
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
}
if (rfm22b_dev->rx_buffer_wr != len) {
return RFM22B_EVENT_FAILURE;
}
// we have a valid received packet
enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE;
if (rfm22b_dev->rx_buffer_wr > 0) {
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
// Check the packet for errors.
if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) {
switch (rfm22b_dev->rx_packet.header.type) {
case PACKET_TYPE_STATUS:
ret_event = RFM22B_EVENT_STATUS_RECEIVED;
break;
case PACKET_TYPE_CON_REQUEST:
ret_event = RFM22B_EVENT_CONNECTION_REQUESTED;
break;
case PACKET_TYPE_DATA:
{
// Send the data to the com port
bool rx_need_yield;
if (rfm22b_dev->rx_in_cb)
(rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield);
#ifdef RFM22B_TEST_DROPPED_PACKETS
// Inject radnom missed ACKs
{
static uint8_t crc = 0;
static uint8_t cntr = 0;
crc = PIOS_CRC_updateByte(crc, cntr++);
if ((crc & 0x7) == 0)
ret_event = RFM22B_EVENT_RX_MODE;
}
#endif
break;
}
case PACKET_TYPE_DUPLICATE_DATA:
break;
case PACKET_TYPE_ACK:
case PACKET_TYPE_ACK_RTS:
ret_event = RFM22B_EVENT_PACKET_ACKED;
break;
case PACKET_TYPE_NACK:
ret_event = RFM22B_EVENT_PACKET_NACKED;
break;
case PACKET_TYPE_PPM:
{
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet);
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
bool ppm_output = false;
#endif
#endif
#if defined(PIOS_INCLUDE_RFM22B_RCVR)
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
rfm22b_dev->ppm_channel[i] = ppmp->channels[i];
}
#endif
#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
if (PIOS_PPM_OUTPUT) {
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
}
}
#endif
#if defined(PIOS_INCLUDE_GCSRCVR)
if (!ppm_output) {
GCSReceiverData gcsRcvr;
for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
gcsRcvr.Channel[i] = ppmp->channels[i];
}
GCSReceiverSet(&gcsRcvr);
}
#endif
break;
}
default:
break;
}
}
else {
ret_event = RFM22B_EVENT_RX_ERROR;
}
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
if (rfm22b_dev->rx_complete_ticks == 0)
rfm22b_dev->rx_complete_ticks = 1;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D2_LED_OFF;
#endif
}
// We're finished with Rx mode
rfm22b_dev->in_rx_mode = false;
// Start a new transaction
rfm22b_dev->packet_start_ticks = 0;
return ret_event;
}
return RFM22B_EVENT_NUM_EVENTS;
return RADIO_EVENT_NUM_EVENTS;
}
/**
* Complete the receipt of a packet.
*
* @param[in] rfm22b_dev The device structure
* @param[in] radio_dev The device structure
* @param[in] p The packet handle of the received packet.
* @param[in] rc_len The number of bytes received.
* @return enum pios_radio_event The next event to inject
*/
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len)
static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, PHPacketHandle p, uint16_t rx_len)
{
portTickType curTicks = xTaskGetTickCount();
// Attempt to correct any errors in the packet.
decode_data((unsigned char*)p, rx_len);
@ -1735,285 +1957,167 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand
corrected_packet = true;
}
// Add any missed packets into the stats.
bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK));
if (!ack_nack_packet && (good_packet || corrected_packet)) {
uint16_t seq_num = p->header.seq_num;
if (rfm22_isConnected(rfm22b_dev)) {
static bool first_time = true;
uint16_t missed_packets = 0;
if (first_time) {
first_time = false;
} else {
uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq;
if (seq_num > prev_seq_num)
missed_packets = seq_num - prev_seq_num - 1;
else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA))
p->header.type = PACKET_TYPE_DUPLICATE_DATA;
}
rfm22b_dev->stats.rx_missed += missed_packets;
}
rfm22b_dev->stats.rx_seq = seq_num;
}
// Set the packet status
if (good_packet) {
rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET);
} else if(corrected_packet) {
// We corrected the error.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET);
} else {
// We couldn't correct the error, so drop the packet.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET);
rfm22b_add_rx_status(radio_dev, RADIO_ERROR_RX_PACKET);
}
return (good_packet || corrected_packet);
}
enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE;
if (good_packet || corrected_packet) {
switch (p->header.type) {
case PACKET_TYPE_STATUS:
ret_event = RADIO_EVENT_STATUS_RECEIVED;
/**
* Start a transmit if possible
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
{
PHPacketHandle p = NULL;
// Don't send if it's not our turn.
if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) {
return RFM22B_EVENT_RX_MODE;
}
// See if there's a packet ready to send.
if (rfm22b_dev->tx_packet) {
p = rfm22b_dev->tx_packet;
} else {
// Don't send a packet if we're waiting for an ACK
if (rfm22b_dev->prev_tx_packet) {
return RFM22B_EVENT_RX_MODE;
}
// Send a connection request?
if (!p && rfm22b_dev->send_connection_request) {
p = (PHPacketHandle)&(rfm22b_dev->con_packet);
rfm22b_dev->send_connection_request = false;
}
#ifdef PIOS_PPM_RECEIVER
// Send a PPM packet?
if (!p && rfm22b_dev->send_ppm) {
p = (PHPacketHandle)&(rfm22b_dev->ppm_packet);
rfm22b_dev->send_ppm = false;
}
#endif
// Send status?
if (!p && rfm22b_dev->send_status) {
p = (PHPacketHandle)&(rfm22b_dev->status_packet);
rfm22b_dev->send_status = false;
}
// Try to get some data to send
if (!p) {
bool need_yield = false;
p = &rfm22b_dev->data_packet;
p->header.type = PACKET_TYPE_DATA;
p->header.destination_id = rfm22b_dev->destination_id;
if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) {
p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
}
// Don't send any data until we're connected.
if (!rfm22_isConnected(rfm22b_dev)) {
p->header.data_size = 0;
}
if (p->header.data_size == 0) {
p = NULL;
}
}
if (p) {
p->header.seq_num = rfm22b_dev->stats.tx_seq++;
}
}
if (!p) {
return RFM22B_EVENT_RX_MODE;
}
// We're transitioning out of Rx mode.
rfm22b_dev->in_rx_mode = false;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D1_LED_ON;
D2_LED_OFF;
#endif
// Change the channel if necessary.
if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) ||
(rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
rfm22_changeChannel(rfm22b_dev);
}
// Add the error correcting code.
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
rfm22b_dev->tx_packet = p;
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0) {
rfm22b_dev->packet_start_ticks = 1;
}
// disable interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
// TUNE mode
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
// Queue the data up for sending
rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
RX_LED_OFF;
// Set the destination address in the transmit header.
// The destination address is the first 4 bytes of the message.
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
// FIFO mode, GFSK modulation
uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
RFM22_mmc2_modtyp_gfsk);
// clear FIFOs
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
// *******************
// add some data to the chips TX FIFO before enabling the transmitter
// set the total number of data bytes we are going to transmit
rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
// add some data
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
// enable TX interrupts
rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
// enable the transmitter
rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
TX_LED_ON;
return RFM22B_EVENT_NUM_EVENTS;
}
/**
* Receive packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
{
enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS;
// Read the device status registers
if (!rfm22_readStatus(rfm22b_dev)) {
return RFM22B_EVENT_FAILURE;
}
// TX FIFO almost empty, it needs filling up
if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) {
// top-up the rf chips TX FIFO buffer
uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write;
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
rfm22b_dev->tx_data_rd += bytes_to_write;
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
// Packet has been sent
} else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) {
portTickType curTicks = xTaskGetTickCount();
rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet);
// Is this an ACK?
bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS));
ret_event = RFM22B_EVENT_RX_MODE;
if (is_ack) {
// If this is an ACK for a connection request message we need to
// configure this modem from the connection request message.
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22_setConnectionParameters(rfm22b_dev);
} else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) {
// Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
// Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) {
PHStatusPacketHandle status = (PHStatusPacketHandle)&(radio_dev->rx_packet);
uint32_t source_id = status->source_id;
for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) {
if (rfm22b_dev->bindings[i].pairID == source_id) {
rfm22b_dev->cur_binding = i;
ret_event = RFM22B_EVENT_REQUEST_CONNECTION;
if (radio_dev->bindings[i].pairID == source_id) {
radio_dev->cur_binding = i;
ret_event = RADIO_EVENT_REQUEST_CONNECTION;
break;
}
}
}
break;
// Change the channel
// On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message.
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
rfm22b_dev->time_delta = portMAX_DELAY - curTicks;
case PACKET_TYPE_CON_REQUEST:
ret_event = RADIO_EVENT_CONNECTION_REQUESTED;
break;
case PACKET_TYPE_DATA:
{
// Send the data to the com port
bool rx_need_yield;
if (radio_dev->rx_in_cb)
(radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield);
break;
}
case PACKET_TYPE_DUPLICATE_DATA:
break;
case PACKET_TYPE_ACK:
ret_event = RADIO_EVENT_PACKET_ACKED;
break;
case PACKET_TYPE_NACK:
ret_event = RADIO_EVENT_PACKET_NACKED;
break;
case PACKET_TYPE_PPM:
{
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
PHPpmPacketHandle ppmp = (PHPpmPacketHandle)p;
#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
bool ppm_output = false;
#endif
#endif
#if defined(PIOS_INCLUDE_RFM22B_RCVR)
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
radio_dev->ppm_channel[i] = ppmp->channels[i];
}
#endif
#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
if (PIOS_PPM_OUTPUT) {
ppm_output = true;
for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
}
}
#endif
#if defined(PIOS_INCLUDE_GCSRCVR)
if (!ppm_output) {
GCSReceiverData gcsRcvr;
for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
gcsRcvr.Channel[i] = ppmp->channels[i];
}
GCSReceiverSet(&gcsRcvr);
}
#endif
break;
}
default:
break;
}
uint16_t seq_num = radio_dev->rx_packet.header.seq_num;
if (rfm22_isConnected(radio_dev)) {
// Adjust the clock syncronization if this is the remote modem.
// The coordinator sends the time that the previous packet was finised transmitting,
// which should match the time that the last packet was received.
uint16_t prev_seq_num = radio_dev->stats.rx_seq;
if (seq_num == (prev_seq_num + 1)) {
portTickType local_rx_time = radio_dev->rx_complete_ticks;
portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time;
radio_dev->time_delta = remote_tx_time - local_rx_time;
} else if (seq_num > prev_seq_num) {
// Add any missed packets into the stats.
uint16_t missed_packets = seq_num - prev_seq_num - 1;
radio_dev->stats.rx_missed += missed_packets;
}
} else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) {
// We need to wait for an ACK if this packet it not an ACK or NACK.
rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet;
rfm22b_dev->tx_complete_ticks = xTaskGetTickCount();
}
// Set the Tx period
if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) {
rfm22b_dev->time_to_send_offset = curTicks + 0x4;
} else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) {
rfm22b_dev->time_to_send_offset = curTicks;
}
rfm22b_dev->tx_packet = 0;
rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
// Start a new transaction
rfm22b_dev->packet_start_ticks = 0;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
D1_LED_OFF;
#endif
// Update the sequence number
radio_dev->stats.rx_seq = seq_num;
} else {
ret_event = RADIO_EVENT_RX_COMPLETE;
}
// Log the time that the packet was received.
radio_dev->rx_complete_ticks = curTicks;
if (radio_dev->rx_complete_ticks == 0)
radio_dev->rx_complete_ticks = 1;
return ret_event;
}
/**
* Receive the packet data.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev)
{
enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev);
switch (res) {
case PIOS_RFM22B_RX_COMPLETE:
// Receive the packet.
ret_event = radio_receivePacket(radio_dev, radio_dev->rx_packet_handle, radio_dev->rx_buffer_wr);
radio_dev->rx_buffer_wr = 0;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_OFF;
#endif
// Start a new transaction
radio_dev->packet_start_ticks = 0;
break;
case PIOS_RFM22B_INT_FAILURE:
ret_event = RADIO_EVENT_RX_MODE;
break;
default:
// do nothing.
break;
}
return ret_event;
}
/*****************************************************************************
* Packet Transmition Functions
@ -2026,8 +2130,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
*/
static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
// The coordinator doesn't send status.
if (rfm22b_dev->coordinator) {
// Don't send if a status is already queued.
if (rfm22b_dev->send_status) {
return;
}
@ -2057,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
@ -2092,46 +2196,44 @@ static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
* Send an ACK to a received packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
{
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks);
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
// We don't ACK PPM or status packets.
if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) {
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = PACKET_TYPE_ACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
}
return RADIO_EVENT_TX_START;
}
/**
* Send an NACK to a received packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
{
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->destination_id;
aph->header.type = PACKET_TYPE_NACK;
aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
rfm22b_dev->tx_packet = (PHPacketHandle)aph;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Send a connection request message.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
{
PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet);
@ -2144,15 +2246,15 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
cph->header.type = PACKET_TYPE_CON_REQUEST;
cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet));
cph->source_id = rfm22b_dev->deviceID;
cph->status_rx_time = rfm22b_dev->rx_complete_ticks;
cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port;
cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port;
cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port;
cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed;
rfm22b_dev->time_to_send = true;
rfm22b_dev->send_connection_request = true;
rfm22b_dev->prev_tx_packet = NULL;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
@ -2164,12 +2266,11 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
* Receive an ACK.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
{
PHPacketHandle prev = rfm22b_dev->prev_tx_packet;
portTickType curTicks = xTaskGetTickCount();
// Clear the previous TX packet.
rfm22b_dev->prev_tx_packet = NULL;
@ -2184,35 +2285,17 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de
break;
}
// On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message.
if (prev->header.type == PACKET_TYPE_CON_REQUEST) {
rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks;
} else if (!rfm22b_dev->coordinator) {
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet));
portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks);
portTickType remote_rx_time = aph->packet_recv_time;
// Adjust the time delta based on the difference between our estimated time offset and the coordinator offset.
rfm22b_dev->time_delta += remote_rx_time - local_tx_time;
}
// Should we try to start another TX?
if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) {
rfm22b_dev->time_to_send_offset = curTicks;
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
} else {
rfm22b_dev->time_to_send_offset = curTicks + 0x4;
return RFM22B_EVENT_RX_MODE;
}
return RADIO_EVENT_TX_START;
}
/**
* Receive an MACK.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
{
// Resend the previous TX packet.
@ -2221,19 +2304,18 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d
// Increment the reset packet counter if we're connected.
if (rfm22_isConnected(rfm22b_dev)) {
rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET);
rfm22b_add_rx_status(rfm22b_dev, RADIO_RESENT_TX_PACKET);
}
rfm22b_dev->time_to_send = true;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Receive a status packet
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
{
PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
int8_t rssi = rfm22b_dev->rssi_dBm;
@ -2272,7 +2354,7 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b
rfm22b_dev->pair_stats[min_idx].lastContact = 0;
}
return RFM22B_EVENT_RX_COMPLETE;
return RADIO_EVENT_RX_COMPLETE;
}
@ -2296,16 +2378,16 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
uint32_t val = rfm22b_dev->rx_packet_stats[i];
for (uint8_t j = 0; j < 16; ++j) {
switch ((val >> (j * 2)) & 0x3) {
case RFM22B_GOOD_RX_PACKET:
case RADIO_GOOD_RX_PACKET:
rfm22b_dev->stats.rx_good++;
break;
case RFM22B_CORRECTED_RX_PACKET:
case RADIO_CORRECTED_RX_PACKET:
rfm22b_dev->stats.rx_corrected++;
break;
case RFM22B_ERROR_RX_PACKET:
case RADIO_ERROR_RX_PACKET:
rfm22b_dev->stats.rx_error++;
break;
case RFM22B_RESENT_TX_PACKET:
case RADIO_RESENT_TX_PACKET:
rfm22b_dev->stats.tx_resent++;
break;
}
@ -2334,39 +2416,6 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r
rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status;
}
/**
* Is it this modem's turn to send?
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev)
{
// Is there a status of PPM packet ready to send?
if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) {
return true;
}
// Are we not connected yet?
if (!rfm22_isConnected(rfm22b_dev)) {
return true;
}
// Is there some data ready to sent?
PHPacketHandle dp = &rfm22b_dev->data_packet;
if (dp->header.data_size > 0) {
return true;
}
bool need_yield = false;
if (rfm22b_dev->tx_out_cb) {
dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield);
}
if (dp->header.data_size > 0) {
return true;
}
return false;
}
/*****************************************************************************
* Connection Handling Functions
@ -2402,16 +2451,16 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev)
// Configure this modem from the connection request message.
rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing);
rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
pios_rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
}
/**
* Accept a connection request.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
{
// Set our connection state to connected
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
@ -2424,7 +2473,13 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
// Set the destination ID to the source of the connection request message.
rfm22b_dev->destination_id = cph->source_id;
return RFM22B_EVENT_DEFAULT;
// The remote modem sets the time delta between the two modems using the differene between the clock
// on the local modem when it sent the status packet and the time on the coordinator modem when it was received.
portTickType local_tx_time = rfm22b_dev->tx_complete_ticks;
portTickType remote_rx_time = cph->status_rx_time;
rfm22b_dev->time_delta = remote_rx_time - local_tx_time;
return RADIO_EVENT_DEFAULT;
}
@ -2432,20 +2487,6 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
* Frequency Hopping Functions
*****************************************************************************/
/**
* There needs to be a buffer in time around a channel change in which we delay starting a new packet transmit.
* This function returns true of we are in that range of time.
*
* @param[in] rfm22b_dev The device structure
* @return True if we're near a channel change time.
*/
static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
{
portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
uint8_t window = (uint8_t)(time & 0x7e);
return ((window == 0x7f) || (window == 0));
}
/**
* Return the extimated current clock ticks count on the coordinator modem.
* This is the master clock used for all synchronization.
@ -2454,9 +2495,26 @@ static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
*/
static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks)
{
if (rfm22b_dev->coordinator) {
return ticks;
}
return ticks + rfm22b_dev->time_delta;
}
/**
* Return true if this modem is in the send interval, which allows the modem to initiate a transmit.
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
{
portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
// Divide time into 8ms blocks. Coordinator sends in firs 2 ms, and remote send in 5th and 6th ms.
bool tts = (rfm22b_dev->coordinator) ? ((time & 0x06) == 0) : (((time + 4) & 0x06) == 0);
// Noone starts a transmit just prior to a channel change.
return tts && ((time & 0x7e) < 0x7b);
}
/**
* Calculate what the current channel shold be.
*
@ -2491,44 +2549,27 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev)
* Error Handling Functions
*****************************************************************************/
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
rfm22b_dev->in_rx_mode = false;
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
return RFM22B_EVENT_RX_MODE;
}
/**
* Recover from a transmit failure.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.tx_failure++;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_TX_START;
}
/**
* Recover from a timeout event.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.timeouts++;
rfm22b_dev->packet_start_ticks = 0;
@ -2536,29 +2577,30 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
if (rfm22b_dev->tx_packet != 0) {
rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
}
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
rfm22b_dev->rx_buffer_wr = 0;
TX_LED_OFF;
RX_LED_OFF;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
D2_LED_OFF;
D3_LED_OFF;
D4_LED_OFF;
#endif
return RFM22B_EVENT_TX_START;
return RADIO_EVENT_RX_MODE;
}
/**
* Recover from a severe error.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.resets++;
rfm22_clearLEDs();
return RFM22B_EVENT_INITIALIZE;
return RADIO_EVENT_INITIALIZE;
}
/**
@ -2566,9 +2608,9 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
* this should not happen.
*
* @parem [in] rfm22b_dev The device structure
* @return enum pios_rfm22b_event The next event to inject
* @return enum pios_radio_event The next event to inject
*/
static enum pios_rfm22b_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();
@ -2592,7 +2634,7 @@ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_d
PIOS_Assert(0);
return RFM22B_EVENT_FATAL_ERROR;
return RADIO_EVENT_FATAL_ERROR;
}
@ -2658,7 +2700,7 @@ static void rfm22_clearLEDs(void) {
LINK_LED_OFF;
RX_LED_OFF;
TX_LED_OFF;
#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D1_LED_OFF;
D2_LED_OFF;
D3_LED_OFF;
@ -2727,7 +2769,7 @@ static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev)
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
{
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
@ -2738,22 +2780,18 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_
}
/**
* Read a byte from an RFM22b register
* Write a byte to a register without claiming the semaphore
*
* @param[in] rfm22b_dev The RFM22B device structure pointer.
* @param[in] addr The address to read from
* @return Returns the result of the register read
* @param[in] rfm22b_dev The RFM22B device.
* @param[in] addr The address to write to
* @param[in] data The datat to write to that address
*/
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
{
uint8_t in[2];
uint8_t out[2] = {addr & 0x7f, 0xFF};
rfm22_claimBus(rfm22b_dev);
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
uint8_t buf[2] = {addr | 0x80, data};
PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
rfm22_deassertCs(rfm22b_dev);
rfm22_releaseBus(rfm22b_dev);
return in[1];
}
/**
@ -2763,7 +2801,7 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
* @param[in] addr The address to read from
* @return Returns the result of the register read
*/
static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
{
uint8_t out[2] = {addr & 0x7F, 0xFF};
uint8_t in[2];

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)) {

12
flight/pios/inc/pios_rfm22b.h
View File

@ -75,6 +75,13 @@ enum rfm22b_datarate {
RFM22_datarate_256000 = 14,
};
typedef enum {
PIOS_RFM22B_INT_FAILURE,
PIOS_RFM22B_INT_SUCCESS,
PIOS_RFM22B_TX_COMPLETE,
PIOS_RFM22B_RX_COMPLETE
} pios_rfm22b_int_result;
struct rfm22b_stats {
uint16_t packets_per_sec;
uint16_t tx_byte_count;
@ -116,7 +123,12 @@ extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id);
extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);
extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs);
extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id);
extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id);
extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p);
extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p);
extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id);
extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id);
/* Global Variables */
extern const struct pios_com_driver pios_rfm22b_com_driver;

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

@ -531,62 +531,69 @@ enum pios_rfm22b_dev_magic {
PIOS_RFM22B_DEV_MAGIC = 0x68e971b6,
};
enum pios_radio_state {
RADIO_STATE_UNINITIALIZED,
RADIO_STATE_INITIALIZING,
RADIO_STATE_REQUESTING_CONNECTION,
RADIO_STATE_ACCEPTING_CONNECTION,
RADIO_STATE_RX_MODE,
RADIO_STATE_RX_DATA,
RADIO_STATE_RX_FAILURE,
RADIO_STATE_RECEIVING_STATUS,
RADIO_STATE_TX_START,
RADIO_STATE_TX_DATA,
RADIO_STATE_TX_FAILURE,
RADIO_STATE_SENDING_ACK,
RADIO_STATE_SENDING_NACK,
RADIO_STATE_RECEIVING_ACK,
RADIO_STATE_RECEIVING_NACK,
RADIO_STATE_TIMEOUT,
RADIO_STATE_ERROR,
RADIO_STATE_FATAL_ERROR,
RADIO_STATE_NUM_STATES // Must be last
};
enum pios_radio_event {
RADIO_EVENT_DEFAULT,
RADIO_EVENT_INT_RECEIVED,
RADIO_EVENT_INITIALIZE,
RADIO_EVENT_INITIALIZED,
RADIO_EVENT_REQUEST_CONNECTION,
RADIO_EVENT_CONNECTION_REQUESTED,
RADIO_EVENT_PACKET_ACKED,
RADIO_EVENT_PACKET_NACKED,
RADIO_EVENT_ACK_TIMEOUT,
RADIO_EVENT_RX_MODE,
RADIO_EVENT_RX_COMPLETE,
RADIO_EVENT_STATUS_RECEIVED,
RADIO_EVENT_TX_START,
RADIO_EVENT_FAILURE,
RADIO_EVENT_TIMEOUT,
RADIO_EVENT_ERROR,
RADIO_EVENT_FATAL_ERROR,
RADIO_EVENT_NUM_EVENTS // Must be last
};
enum pios_rfm22b_state {
RFM22B_STATE_UNINITIALIZED,
RFM22B_STATE_INITIALIZING,
RFM22B_STATE_REQUESTING_CONNECTION,
RFM22B_STATE_ACCEPTING_CONNECTION,
RFM22B_STATE_TRANSITION,
RFM22B_STATE_RX_WAIT,
RFM22B_STATE_RX_WAIT_SYNC,
RFM22B_STATE_RX_MODE,
RFM22B_STATE_WAIT_PREAMBLE,
RFM22B_STATE_WAIT_SYNC,
RFM22B_STATE_RX_DATA,
RFM22B_STATE_RX_FAILURE,
RFM22B_STATE_RECEIVING_STATUS,
RFM22B_STATE_TX_START,
RFM22B_STATE_TX_DATA,
RFM22B_STATE_TX_FAILURE,
RFM22B_STATE_SENDING_ACK,
RFM22B_STATE_SENDING_NACK,
RFM22B_STATE_RECEIVING_ACK,
RFM22B_STATE_RECEIVING_NACK,
RFM22B_STATE_TIMEOUT,
RFM22B_STATE_ERROR,
RFM22B_STATE_FATAL_ERROR,
RFM22B_STATE_TX_MODE,
RFM22B_STATE_TRANSMITTING,
RFM22B_STATE_NUM_STATES // Must be last
};
enum pios_rfm22b_event {
RFM22B_EVENT_DEFAULT,
RFM22B_EVENT_INT_RECEIVED,
RFM22B_EVENT_INITIALIZE,
RFM22B_EVENT_INITIALIZED,
RFM22B_EVENT_REQUEST_CONNECTION,
RFM22B_EVENT_CONNECTION_REQUESTED,
RFM22B_EVENT_PACKET_ACKED,
RFM22B_EVENT_PACKET_NACKED,
RFM22B_EVENT_ACK_TIMEOUT,
RFM22B_EVENT_RX_MODE,
RFM22B_EVENT_PREAMBLE_DETECTED,
RFM22B_EVENT_SYNC_DETECTED,
RFM22B_EVENT_RX_COMPLETE,
RFM22B_EVENT_RX_ERROR,
RFM22B_EVENT_STATUS_RECEIVED,
RFM22B_EVENT_TX_START,
RFM22B_EVENT_FAILURE,
RFM22B_EVENT_TIMEOUT,
RFM22B_EVENT_ERROR,
RFM22B_EVENT_FATAL_ERROR,
RFM22B_EVENT_NUM_EVENTS // Must be last
};
#define RFM22B_RX_PACKET_STATS_LEN 4
enum pios_rfm22b_rx_packet_status {
RFM22B_GOOD_RX_PACKET = 0x00,
RFM22B_CORRECTED_RX_PACKET = 0x01,
RFM22B_ERROR_RX_PACKET = 0x2,
RFM22B_RESENT_TX_PACKET = 0x3
RADIO_GOOD_RX_PACKET = 0x00,
RADIO_CORRECTED_RX_PACKET = 0x01,
RADIO_ERROR_RX_PACKET = 0x2,
RADIO_RESENT_TX_PACKET = 0x3
};
typedef struct {
@ -604,6 +611,79 @@ typedef struct {
OPLinkSettingsComSpeedOptions com_speed;
} rfm22b_binding;
enum pios_rfm22b_chip_power_state {
RFM22B_IDLE_STATE = 0x00,
RFM22B_RX_STATE = 0x01,
RFM22B_TX_STATE = 0x10,
RFM22B_INVALID_STATE = 0x11
};
// Device Status
typedef union {
struct {
uint8_t state : 2;
bool frequency_error : 1;
bool header_error : 1;
bool rx_fifo_empty : 1;
bool fifo_underflow : 1;
bool fifo_overflow : 1;
};
uint8_t raw;
} rfm22b_device_status_reg;
// EzMAC Status
typedef union {
struct {
bool packet_sent : 1;
bool packet_transmitting : 1;
bool crc_error : 1;
bool valid_packet_received : 1;
bool packet_receiving : 1;
bool packet_searching : 1;
bool crc_is_all_ones : 1;
bool reserved;
};
uint8_t raw;
} rfm22b_ezmac_status_reg;
// Interrrupt Status Register 1
typedef union {
struct {
bool crc_error : 1;
bool valid_packet_received : 1;
bool packet_sent_interrupt : 1;
bool external_interrupt : 1;
bool rx_fifo_almost_full : 1;
bool tx_fifo_almost_empty : 1;
bool tx_fifo_almost_full : 1;
bool fifo_underoverflow_error : 1;
};
uint8_t raw;
} rfm22b_int_status_1;
// Interrupt Status Register 2
typedef union {
struct {
bool poweron_reset : 1;
bool chip_ready : 1;
bool low_battery_detect : 1;
bool wakeup_timer : 1;
bool rssi_above_threshold : 1;
bool invalid_preamble_detected : 1;
bool valid_preamble_detected : 1;
bool sync_word_detected : 1;
};
uint8_t raw;
} rfm22b_int_status_2;
typedef struct {
rfm22b_device_status_reg device_status;
rfm22b_device_status_reg ezmac_status;
rfm22b_int_status_1 int_status_1;
rfm22b_int_status_2 int_status_2;
} rfm22b_device_status;
struct pios_rfm22b_dev {
enum pios_rfm22b_dev_magic magic;
struct pios_rfm22b_cfg cfg;
@ -644,30 +724,27 @@ struct pios_rfm22b_dev {
uint32_t tx_out_context;
// the transmit power to use for data transmissions
uint8_t tx_power;
uint8_t tx_power;
// The RF datarate lookup index.
uint8_t datarate;
// The state machine state and the current event
enum pios_rfm22b_state state;
// The radio state machine state
enum pios_radio_state state;
// The event queue handle
xQueueHandle eventQueue;
// device status register
uint8_t device_status;
// interrupt status register 1
uint8_t int_status1;
// interrupt status register 2
uint8_t int_status2;
// ezmac status register
uint8_t ezmac_status;
// The device status registers.
rfm22b_device_status status_regs;
// The error statistics counters
uint16_t prev_rx_seq_num;
uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN];
// The RFM22B state machine state
enum pios_rfm22b_state rfm22b_state;
// The packet statistics
struct rfm22b_stats stats;
@ -692,12 +769,12 @@ struct pios_rfm22b_dev {
// The rx data packet
PHPacket rx_packet;
// The rx data packet
PHPacketHandle rx_packet_handle;
// The receive buffer write index
uint16_t rx_buffer_wr;
// The receive buffer write index
uint16_t rx_packet_len;
// Is the modem currently in Rx mode?
bool in_rx_mode;
// The status packet
PHStatusPacket status_packet;
@ -717,10 +794,6 @@ struct pios_rfm22b_dev {
bool send_status;
bool send_ppm;
bool send_connection_request;
bool time_to_send;
// The offset between our clock and the global send clock
uint8_t time_to_send_offset;
// The initial frequency
uint32_t init_frequency;
@ -730,7 +803,7 @@ struct pios_rfm22b_dev {
// The frequency hopping step size
float frequency_step_size;
// current frequency hop channel
uint8_t frequency_hop_channel;
uint8_t frequency_hop_channel;
// afc correction reading (in Hz)
int8_t afc_correction_Hz;
@ -749,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;

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