diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 7fc0ed5c2..1bac5f502 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -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; diff --git a/flight/libraries/WorldMagModel.c b/flight/libraries/WorldMagModel.c index ae5becc74..af50c8a19 100644 --- a/flight/libraries/WorldMagModel.c +++ b/flight/libraries/WorldMagModel.c @@ -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++) diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index 562937c75..ea8e11e23 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -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); diff --git a/flight/libraries/inc/packet_handler.h b/flight/libraries/inc/packet_handler.h index 5e04884ad..855347a20 100644 --- a/flight/libraries/inc/packet_handler.h +++ b/flight/libraries/inc/packet_handler.h @@ -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__ diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index 051338572..7eca68a51 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -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;ilastErr); 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) diff --git a/flight/libraries/op_dfu.c b/flight/libraries/op_dfu.c index 60fe8fe86..786ce1910 100644 --- a/flight/libraries/op_dfu.c +++ b/flight/libraries/op_dfu.c @@ -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; diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 21151548e..4b9394672 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -24,7 +24,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "pios.h" +#include +#include + #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) { diff --git a/flight/libraries/printf2.c b/flight/libraries/printf2.c index 775fb294e..dee058976 100644 --- a/flight/libraries/printf2.c +++ b/flight/libraries/printf2.c @@ -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�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++ = '+' ; diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index b01a753eb..8ee52fce9 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -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; } diff --git a/flight/modules/Airspeed/revolution/airspeed.c b/flight/modules/Airspeed/revolution/airspeed.c index fd1479d0f..5703f33a8 100644 --- a/flight/modules/Airspeed/revolution/airspeed.c +++ b/flight/modules/Airspeed/revolution/airspeed.c @@ -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); diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c index 5da2e2810..3f6012c30 100644 --- a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c +++ b/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -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; diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index 04c2ac48c..f848d8931 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -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; diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 1cee36418..a97e4bd2c 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -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; diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index ed3b0bcf6..129875b60 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -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); } diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 06b9a42c2..74137c001 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -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; diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 922ec269e..be2534b2a 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -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); } /** * @} diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c index 260f70750..0570f7016 100644 --- a/flight/modules/Autotune/autotune.c +++ b/flight/modules/Autotune/autotune.c @@ -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); diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index ac9dcace3..c0f6db89e 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -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; diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 7aed45983..48f7a308f 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -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); } diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 13c9f6ee4..7fd83b920 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -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++; } diff --git a/flight/modules/Example/examplemodcallback.c b/flight/modules/Example/examplemodcallback.c index 1a050f07e..6faba152f 100644 --- a/flight/modules/Example/examplemodcallback.c +++ b/flight/modules/Example/examplemodcallback.c @@ -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); } diff --git a/flight/modules/Example/examplemodperiodic.c b/flight/modules/Example/examplemodperiodic.c index abb2484f7..8bfe98211 100644 --- a/flight/modules/Example/examplemodperiodic.c +++ b/flight/modules/Example/examplemodperiodic.c @@ -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; diff --git a/flight/modules/Example/examplemodthread.c b/flight/modules/Example/examplemodthread.c index d8d881a5d..813a437e4 100644 --- a/flight/modules/Example/examplemodthread.c +++ b/flight/modules/Example/examplemodthread.c @@ -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; diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 99df558fe..a706940b2 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -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); diff --git a/flight/modules/FirmwareIAP/firmwareiap.c b/flight/modules/FirmwareIAP/firmwareiap.c index f8e6fd231..f0cc119c5 100644 --- a/flight/modules/FirmwareIAP/firmwareiap.c +++ b/flight/modules/FirmwareIAP/firmwareiap.c @@ -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); diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index daff8f5ca..8f4dcc049 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -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; diff --git a/flight/modules/FlightPlan/flightplan.c b/flight/modules/FlightPlan/flightplan.c index e919dd41e..7c3b58027 100644 --- a/flight/modules/FlightPlan/flightplan.c +++ b/flight/modules/FlightPlan/flightplan.c @@ -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; diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 92b72602e..ec2d79d3f 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -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); diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 17b1678ba..98cfe48cc 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -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; diff --git a/flight/modules/MK/MKSerial/MKSerial.c b/flight/modules/MK/MKSerial/MKSerial.c index 3c8972711..ed1cb7006 100644 --- a/flight/modules/MK/MKSerial/MKSerial.c +++ b/flight/modules/MK/MKSerial/MKSerial.c @@ -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; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 479ea221c..7e248ae0c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -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(); } diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 26be45c62..3c8cec1eb 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -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 diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index a1e4a5d69..2c806ad64 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -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; } diff --git a/flight/modules/Osd/WavPlayer/wavplayer.c b/flight/modules/Osd/WavPlayer/wavplayer.c index 1788f0a97..2bf8949ea 100644 --- a/flight/modules/Osd/WavPlayer/wavplayer.c +++ b/flight/modules/Osd/WavPlayer/wavplayer.c @@ -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(); diff --git a/flight/modules/Osd/osdgen/inc/osdgen.h b/flight/modules/Osd/osdgen/inc/osdgen.h index 92f8d6cd3..ac5621f51 100644 --- a/flight/modules/Osd/osdgen/inc/osdgen.h +++ b/flight/modules/Osd/osdgen/inc/osdgen.h @@ -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. diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 9172cc006..575cb38e8 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -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 diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index abde6bd22..3d846946e 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -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; diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 5285a88a8..5b329831e 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -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; diff --git a/flight/modules/OveroSync/overosync.c b/flight/modules/OveroSync/overosync.c index 639082fce..9d1bf850d 100644 --- a/flight/modules/OveroSync/overosync.c +++ b/flight/modules/OveroSync/overosync.c @@ -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; diff --git a/flight/modules/OveroSync/simulated/overosync.c b/flight/modules/OveroSync/simulated/overosync.c index a8bf6c769..8b8808895 100644 --- a/flight/modules/OveroSync/simulated/overosync.c +++ b/flight/modules/OveroSync/simulated/overosync.c @@ -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; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 32f99552a..34b36220e 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -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; diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index b4653dabc..8e047b3b8 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -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) { diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 5db0086e9..16149d99f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -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; diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/sensors.c index fef692598..b3e27c986 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/sensors.c @@ -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; diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index 02081f427..be22585ed 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -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(); diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d9a1c3891..412aaea44 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -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 diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index c123174b3..0b9412081 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -31,6 +31,7 @@ */ #include "openpilot.h" +#include #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]; diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 6ef2e9445..e561b46eb 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -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(¤tHwSettings); @@ -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 diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 2a4927d6b..f57ad32d7 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -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; diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index fd350c0d4..4c784894b 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -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; } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index a80d7e750..61ccf19f1 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -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); } diff --git a/flight/pios/common/libraries/dosfs/dosfs.c b/flight/pios/common/libraries/dosfs/dosfs.c index 4c35f5e77..7fc5f164f 100644 --- a/flight/pios/common/libraries/dosfs/dosfs.c +++ b/flight/pios/common/libraries/dosfs/dosfs.c @@ -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; } diff --git a/flight/pios/common/libraries/msheap/msheap.c b/flight/pios/common/libraries/msheap/msheap.c index 8c86eb45f..2c8c082ba 100644 --- a/flight/pios/common/libraries/msheap/msheap.c +++ b/flight/pios/common/libraries/msheap/msheap.c @@ -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 (;;) ; diff --git a/flight/pios/common/libraries/msheap/pios_msheap.c b/flight/pios/common/libraries/msheap/pios_msheap.c index 7fca3a8b5..e8426f7eb 100644 --- a/flight/pios/common/libraries/msheap/pios_msheap.c +++ b/flight/pios/common/libraries/msheap/pios_msheap.c @@ -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); } diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c index 3f3ba88f8..f8e23e8d3 100644 --- a/flight/pios/common/pios_adxl345.c +++ b/flight/pios/common/pios_adxl345.c @@ -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; diff --git a/flight/pios/common/pios_bma180.c b/flight/pios/common/pios_bma180.c index a5f64a121..35fd09d2f 100644 --- a/flight/pios/common/pios_bma180.c +++ b/flight/pios/common/pios_bma180.c @@ -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 diff --git a/flight/pios/common/pios_bmp085.c b/flight/pios/common/pios_bmp085.c index 0b1201caf..83a77e95e 100644 --- a/flight/pios/common/pios_bmp085.c +++ b/flight/pios/common/pios_bmp085.c @@ -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); diff --git a/flight/pios/common/pios_com_msg.c b/flight/pios/common/pios_com_msg.c index afc68c1ec..91e285a3d 100644 --- a/flight/pios/common/pios_com_msg.c +++ b/flight/pios/common/pios_com_msg.c @@ -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; diff --git a/flight/pios/common/pios_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index 935861484..a05431e19 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -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; } diff --git a/flight/pios/common/pios_flashfs_logfs.c b/flight/pios/common/pios_flashfs_logfs.c index 6f7b59fae..bff3279a8 100644 --- a/flight/pios/common/pios_flashfs_logfs.c +++ b/flight/pios/common/pios_flashfs_logfs.c @@ -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; diff --git a/flight/pios/common/pios_gcsrcvr.c b/flight/pios/common/pios_gcsrcvr.c index 89998a5bc..e7c281d9f 100644 --- a/flight/pios/common/pios_gcsrcvr.c +++ b/flight/pios/common/pios_gcsrcvr.c @@ -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 */ diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 37a01f5e4..2b2d06b0b 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -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; diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 190ca883f..203d8b1bb 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -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]; diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index 11081b11d..e0800eb27 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -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)) { diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index 5c0514a8b..b1ce7a24b 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -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; diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 584c354d5..7cecb0a36 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -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 }; diff --git a/flight/pios/pios_sim_posix.h b/flight/pios/pios_sim_posix.h index f7ac600bd..d1ba3e22b 100644 --- a/flight/pios/pios_sim_posix.h +++ b/flight/pios/pios_sim_posix.h @@ -58,6 +58,12 @@ #include #include #include +/* 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 #include #include diff --git a/flight/pios/posix/pios_sys.c b/flight/pios/posix/pios_sys.c index f4197f74e..d4645139b 100644 --- a/flight/pios/posix/pios_sys.c +++ b/flight/pios/posix/pios_sys.c @@ -28,7 +28,7 @@ /* Project Includes */ -#include "pios.h" +#include #if defined(PIOS_INCLUDE_SYS) diff --git a/flight/pios/stm32f10x/pios_debug.c b/flight/pios/stm32f10x/pios_debug.c index e7e7e148e..f0f6dbcaf 100644 --- a/flight/pios/stm32f10x/pios_debug.c +++ b/flight/pios/stm32f10x/pios_debug.c @@ -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 diff --git a/flight/pios/stm32f10x/pios_eeprom.c b/flight/pios/stm32f10x/pios_eeprom.c index a3e11c544..8771a522a 100644 --- a/flight/pios/stm32f10x/pios_eeprom.c +++ b/flight/pios/stm32f10x/pios_eeprom.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_led.c b/flight/pios/stm32f10x/pios_led.c index 79dad8a8c..c9d26da8a 100644 --- a/flight/pios/stm32f10x/pios_led.c +++ b/flight/pios/stm32f10x/pios_led.c @@ -34,7 +34,7 @@ #include -const static struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg * led_cfg; /** * Initialises all the LED's diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index f9d825162..2088e426e 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c index 72dbf3056..1352d38d2 100644 --- a/flight/pios/stm32f10x/pios_ppm_out.c +++ b/flight/pios/stm32f10x/pios_ppm_out.c @@ -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)) diff --git a/flight/pios/stm32f10x/pios_pwm.c b/flight/pios/stm32f10x/pios_pwm.c index 2c6707664..d93d37ebc 100644 --- a/flight/pios/stm32f10x/pios_pwm.c +++ b/flight/pios/stm32f10x/pios_pwm.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_rtc.c b/flight/pios/stm32f10x/pios_rtc.c index 77f67a2f4..9d8f45b66 100644 --- a/flight/pios/stm32f10x/pios_rtc.c +++ b/flight/pios/stm32f10x/pios_rtc.c @@ -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() diff --git a/flight/pios/stm32f10x/pios_spi.c b/flight/pios/stm32f10x/pios_spi.c index fcb46ee9a..4e001b775 100644 --- a/flight/pios/stm32f10x/pios_spi.c +++ b/flight/pios/stm32f10x/pios_spi.c @@ -39,7 +39,7 @@ #include -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); diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 3054c09e3..3691c0dfd 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_usb.c b/flight/pios/stm32f10x/pios_usb.c index 6a5d676e5..13fb8c3c5 100644 --- a/flight/pios/stm32f10x/pios_usb.c +++ b/flight/pios/stm32f10x/pios_usb.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_usb_cdc.c b/flight/pios/stm32f10x/pios_usb_cdc.c index 0da873966..2d470dee9 100644 --- a/flight/pios/stm32f10x/pios_usb_cdc.c +++ b/flight/pios/stm32f10x/pios_usb_cdc.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_usb_hid.c b/flight/pios/stm32f10x/pios_usb_hid.c index ba49ceb5a..2656c3dce 100644 --- a/flight/pios/stm32f10x/pios_usb_hid.c +++ b/flight/pios/stm32f10x/pios_usb_hid.c @@ -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; diff --git a/flight/pios/stm32f10x/pios_usb_hid_pwr.c b/flight/pios/stm32f10x/pios_usb_hid_pwr.c index e55e1104e..888f2d748 100644 --- a/flight/pios/stm32f10x/pios_usb_hid_pwr.c +++ b/flight/pios/stm32f10x/pios_usb_hid_pwr.c @@ -44,7 +44,7 @@ struct { * Output : None. * Return : None *******************************************************************************/ -void USB_Cable_Config(FunctionalState NewState) +void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState) { } diff --git a/flight/pios/stm32f10x/pios_usbhook.c b/flight/pios/stm32f10x/pios_usbhook.c index 0fd6e2e5b..ba01d8fee 100644 --- a/flight/pios/stm32f10x/pios_usbhook.c +++ b/flight/pios/stm32f10x/pios_usbhook.c @@ -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; diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_core.c b/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_core.c index 2a51d3aef..91025c4db 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_core.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_Device_Library/Core/src/usbd_core.c @@ -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 */ diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h index 9df1a4172..d8be8b19f 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/inc/usb_dcd_int.h @@ -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; diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c index 74e432ac2..1b6cb84b4 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c @@ -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); diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd.c index d323e7dcb..6c08b1d3d 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd.c @@ -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; diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c index bb1c609f8..8dd7c71a4 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c @@ -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); diff --git a/flight/pios/stm32f4xx/pios_adc.c b/flight/pios/stm32f4xx/pios_adc.c index 7131e440a..1f26dee4e 100644 --- a/flight/pios/stm32f4xx/pios_adc.c +++ b/flight/pios/stm32f4xx/pios_adc.c @@ -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; } diff --git a/flight/pios/stm32f4xx/pios_debug.c b/flight/pios/stm32f4xx/pios_debug.c index 8fe050b07..af10d3918 100644 --- a/flight/pios/stm32f4xx/pios_debug.c +++ b/flight/pios/stm32f4xx/pios_debug.c @@ -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 diff --git a/flight/pios/stm32f4xx/pios_flash_internal.c b/flight/pios/stm32f4xx/pios_flash_internal.c index fceae8038..c106573b3 100644 --- a/flight/pios/stm32f4xx/pios_flash_internal.c +++ b/flight/pios/stm32f4xx/pios_flash_internal.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 9ceb72337..9dec896da 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -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) { diff --git a/flight/pios/stm32f4xx/pios_led.c b/flight/pios/stm32f4xx/pios_led.c index 7ff5dec42..b0d6a4f88 100644 --- a/flight/pios/stm32f4xx/pios_led.c +++ b/flight/pios/stm32f4xx/pios_led.c @@ -34,7 +34,7 @@ #include -const static struct pios_led_cfg * led_cfg; +static const struct pios_led_cfg * led_cfg; /** * Initialises all the LED's diff --git a/flight/pios/stm32f4xx/pios_overo.c b/flight/pios/stm32f4xx/pios_overo.c index 2de75591a..609436295 100644 --- a/flight/pios/stm32f4xx/pios_overo.c +++ b/flight/pios/stm32f4xx/pios_overo.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 922e7d6e1..fad9a22e3 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_pwm.c b/flight/pios/stm32f4xx/pios_pwm.c index 752dcbaa8..9c55cc624 100644 --- a/flight/pios/stm32f4xx/pios_pwm.c +++ b/flight/pios/stm32f4xx/pios_pwm.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_rtc.c b/flight/pios/stm32f4xx/pios_rtc.c index e5b2e64ab..12cf4b4ae 100644 --- a/flight/pios/stm32f4xx/pios_rtc.c +++ b/flight/pios/stm32f4xx/pios_rtc.c @@ -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() diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index bf82a9951..56134c165 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -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); diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 8080cbb69..daa4d25bb 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_usb.c b/flight/pios/stm32f4xx/pios_usb.c index 50ae0837f..d0592a815 100644 --- a/flight/pios/stm32f4xx/pios_usb.c +++ b/flight/pios/stm32f4xx/pios_usb.c @@ -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; diff --git a/flight/pios/stm32f4xx/pios_usb_hid.c b/flight/pios/stm32f4xx/pios_usb_hid.c index 88f45978b..4ef149297 100644 --- a/flight/pios/stm32f4xx/pios_usb_hid.c +++ b/flight/pios/stm32f4xx/pios_usb_hid.c @@ -261,7 +261,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { } } -static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail) +static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail) { struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id; @@ -428,7 +428,7 @@ static bool PIOS_USB_HID_IF_Setup(uint32_t usb_hid_id, struct usb_setup_request return true; } -static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_request *req) +static void PIOS_USB_HID_IF_CtrlDataOut(__attribute__((unused)) uint32_t usb_hid_id, __attribute__((unused)) struct usb_setup_request *req) { /* HID devices don't have any OUT data stages on the control endpoint */ PIOS_Assert(0); @@ -438,7 +438,7 @@ static void PIOS_USB_HID_IF_CtrlDataOut(uint32_t usb_hid_id, struct usb_setup_re * @brief Callback used to indicate a transmission from device INto host completed * Checks if any data remains, pads it into HID packet and sends. */ -static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len) +static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, __attribute__((unused)) uint16_t len) { struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; @@ -460,7 +460,7 @@ static bool PIOS_USB_HID_EP_IN_Callback(uint32_t usb_hid_id, uint8_t epnum, uint /** * EP1 OUT Callback Routine */ -static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, uint8_t epnum, uint16_t len) +static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, __attribute__((unused)) uint8_t epnum, uint16_t len) { struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usb_hid_id; diff --git a/flight/pios/stm32f4xx/pios_usbhook.c b/flight/pios/stm32f4xx/pios_usbhook.c index fa7b8284b..df722b0c0 100644 --- a/flight/pios/stm32f4xx/pios_usbhook.c +++ b/flight/pios/stm32f4xx/pios_usbhook.c @@ -69,7 +69,7 @@ void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * static struct pios_usbhook_descriptor Config_Descriptor; -void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +void PIOS_USBHOOK_RegisterConfig(__attribute__((unused)) uint8_t config_id, const uint8_t * desc, uint16_t desc_size) { Config_Descriptor.descriptor = desc; Config_Descriptor.length = desc_size; @@ -205,42 +205,42 @@ void PIOS_USBHOOK_EndpointRx(uint8_t epnum, uint8_t *buf, uint16_t len) * Device level hooks into STM USB library */ -static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetDeviceDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = Device_Descriptor.length; return Device_Descriptor.descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetLangIDStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = String_Descriptor[USB_STRING_DESC_LANG].length; return String_Descriptor[USB_STRING_DESC_LANG].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetManufacturerStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = String_Descriptor[USB_STRING_DESC_VENDOR].length; return String_Descriptor[USB_STRING_DESC_VENDOR].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetProductStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = String_Descriptor[USB_STRING_DESC_PRODUCT].length; return String_Descriptor[USB_STRING_DESC_PRODUCT].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetSerialStrDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = String_Descriptor[USB_STRING_DESC_SERIAL].length; return String_Descriptor[USB_STRING_DESC_SERIAL].descriptor; } -static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetConfigurationStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { return NULL; } -static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_DEV_GetInterfaceStrDescriptor(__attribute__((unused)) uint8_t speed, __attribute__((unused)) uint16_t *length) { return NULL; } @@ -266,7 +266,7 @@ static void PIOS_USBHOOK_USR_Init(void) #endif } -static void PIOS_USBHOOK_USR_DeviceReset(uint8_t speed) +static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed) { PIOS_USB_ChangeConnectionState(false); } @@ -306,7 +306,7 @@ static USBD_Usr_cb_TypeDef user_callbacks = { .DeviceDisconnected = PIOS_USBHOOK_USR_DeviceDisconnected, }; -static uint8_t PIOS_USBHOOK_CLASS_Init(void *pdev, uint8_t cfgidx) +static uint8_t PIOS_USBHOOK_CLASS_Init(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { /* Call all of the registered init callbacks */ for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { @@ -318,7 +318,7 @@ static uint8_t PIOS_USBHOOK_CLASS_Init(void *pdev, uint8_t cfgidx) return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_DeInit(void *pdev, uint8_t cfgidx) +static uint8_t PIOS_USBHOOK_CLASS_DeInit(__attribute__((unused)) void *pdev, __attribute__((unused)) uint8_t cfgidx) { /* Call all of the registered deinit callbacks */ for (uint8_t i = 0; i < NELEMENTS(usb_if_table); i++) { @@ -331,7 +331,7 @@ static uint8_t PIOS_USBHOOK_CLASS_DeInit(void *pdev, uint8_t cfgidx) } static struct usb_setup_request usb_ep0_active_req; -static uint8_t PIOS_USBHOOK_CLASS_Setup(void *pdev, USB_SETUP_REQ *req) +static uint8_t PIOS_USBHOOK_CLASS_Setup(__attribute__((unused)) void *pdev, USB_SETUP_REQ *req) { switch (req->bmRequest & (USB_REQ_TYPE_MASK | USB_REQ_RECIPIENT_MASK)) { case (USB_REQ_TYPE_STANDARD | USB_REQ_RECIPIENT_INTERFACE): @@ -365,12 +365,12 @@ static uint8_t PIOS_USBHOOK_CLASS_Setup(void *pdev, USB_SETUP_REQ *req) return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(void *pdev) +static uint8_t PIOS_USBHOOK_CLASS_EP0_TxSent(__attribute__((unused)) void *pdev) { return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(void *pdev) +static uint8_t PIOS_USBHOOK_CLASS_EP0_RxReady(__attribute__((unused)) void *pdev) { uint8_t ifnum = LOBYTE(usb_ep0_active_req.wIndex); @@ -418,22 +418,22 @@ static uint8_t PIOS_USBHOOK_CLASS_DataOut(void *pdev, uint8_t epnum) return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_SOF(void *pdev) +static uint8_t PIOS_USBHOOK_CLASS_SOF(__attribute__((unused)) void *pdev) { return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(void *pdev) +static uint8_t PIOS_USBHOOK_CLASS_IsoINIncomplete(__attribute__((unused)) void *pdev) { return USBD_OK; } -static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(void *pdev) +static uint8_t PIOS_USBHOOK_CLASS_IsoOUTIncomplete(__attribute__((unused)) void *pdev) { return USBD_OK; } -static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(uint8_t speed, uint16_t *length) +static const uint8_t * PIOS_USBHOOK_CLASS_GetConfigDescriptor(__attribute__((unused)) uint8_t speed, uint16_t *length) { *length = Config_Descriptor.length; return Config_Descriptor.descriptor; diff --git a/flight/pios/stm32f4xx/startup.c b/flight/pios/stm32f4xx/startup.c index 94a3debcd..933481714 100644 --- a/flight/pios/stm32f4xx/startup.c +++ b/flight/pios/stm32f4xx/startup.c @@ -48,7 +48,7 @@ extern char _sidata, _sdata, _edata, _sfast, _efast; char irq_stack[1024] __attribute__((section(".irqstack"))); /** exception handler */ -typedef const void (vector)(void); +typedef void (vector)(void); /** CortexM3 CPU vectors */ struct cm3_vectors { @@ -145,4 +145,4 @@ struct cm3_vectors cpu_vectors __attribute((section(".cpu_vectors"))) = { /** * @} - */ \ No newline at end of file + */ diff --git a/flight/pios/stm32f4xx/vectors_stm32f4xx.c b/flight/pios/stm32f4xx/vectors_stm32f4xx.c index 2b28d558c..f1a958010 100644 --- a/flight/pios/stm32f4xx/vectors_stm32f4xx.c +++ b/flight/pios/stm32f4xx/vectors_stm32f4xx.c @@ -26,7 +26,7 @@ */ /** interrupt handler */ -typedef const void (vector)(void); +typedef void (vector)(void); /** default interrupt handler */ static void @@ -209,4 +209,4 @@ vector *io_vectors[] __attribute__((section(".io_vectors"))) = { /** * @} - */ \ No newline at end of file + */ diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 92ad35f7b..9b19be3a7 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -210,7 +210,7 @@ extern uint32_t pios_com_hkosd_id; // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16 // ADCCLK = PCLK2 / 2 -#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) #define PIOS_ADC_MAX_OVERSAMPLING 36 //------------------------ diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index e0926a433..ad8b4eacc 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -92,7 +92,7 @@ static const struct pios_led_cfg pios_led_cfg = { .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) { return &pios_led_cfg; } @@ -268,7 +268,7 @@ struct pios_rfm22b_cfg pios_rfm22b_cfg = { }; //! Compatibility layer for various hardware revisions -const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision) +const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (__attribute__((unused)) uint32_t board_revision) { return &pios_rfm22b_cfg; } diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index 390e672e7..7b433a030 100644 --- a/flight/targets/boards/osd/board_hw_defs.c +++ b/flight/targets/boards/osd/board_hw_defs.c @@ -62,7 +62,7 @@ static const struct pios_led_cfg pios_led_cfg = { .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) { return &pios_led_cfg; } @@ -712,7 +712,7 @@ static const struct pios_video_cfg pios_video_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .rx = {}, + /*.rx = {},*/ .tx = { .channel = DMA1_Stream7, .init = { @@ -753,7 +753,7 @@ static const struct pios_video_cfg pios_video_cfg = { .GPIO_PuPd = GPIO_PuPd_NOPULL }, }, - .mosi = {}, + /*.mosi = {},*/ .slave_count = 1, }, .level = { @@ -781,7 +781,7 @@ static const struct pios_video_cfg pios_video_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .rx = {}, + /*.rx = {},*/ .tx = { .channel = DMA2_Stream5, .init = { @@ -822,7 +822,7 @@ static const struct pios_video_cfg pios_video_cfg = { .GPIO_PuPd = GPIO_PuPd_UP }, }, - .mosi = {}, + /*.mosi = {},*/ .slave_count = 1, }, diff --git a/flight/targets/boards/osd/firmware/fonts.c b/flight/targets/boards/osd/firmware/fonts.c index d7fe657e1..5b0c22d75 100644 --- a/flight/targets/boards/osd/firmware/fonts.c +++ b/flight/targets/boards/osd/firmware/fonts.c @@ -31,5 +31,5 @@ struct FontEntry fonts[NUM_FONTS + 1] = { FONT_UPPERCASE_ONLY }, { 2, 8, 10, "font8x10", 0, 0, 0 }, { 3, 12, 18, "font12x18", 0, 0, 0 }, - { -1 } // ends font table + { -1, -1, -1, "", 0, 0, 0 } // ends font table }; diff --git a/flight/targets/boards/osd/firmware/inc/splash.h b/flight/targets/boards/osd/firmware/inc/splash.h index 6cbd41357..3ea31872c 100644 --- a/flight/targets/boards/osd/firmware/inc/splash.h +++ b/flight/targets/boards/osd/firmware/inc/splash.h @@ -10,7 +10,7 @@ #define oplogo_width 144 #define oplogo_height 144 -const static unsigned short oplogo_bits[] = { +static const unsigned short oplogo_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, @@ -156,7 +156,7 @@ const static unsigned short oplogo_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000 }; -const static unsigned short oplogo_mask_bits[] = { +static const unsigned short oplogo_mask_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x003f, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0xc000, 0x03ff, 0x0000, 0x0000, 0x0000, @@ -305,7 +305,7 @@ const static unsigned short oplogo_mask_bits[] = { #define level_width 144 #define level_height 129 -const static unsigned short level_bits[] = { +static const unsigned short level_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, @@ -438,7 +438,7 @@ const static unsigned short level_bits[] = { #define level_mask_width 144 #define level_mask_height 129 -const static unsigned short level_mask_bits[] = { +static const unsigned short level_mask_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, @@ -571,7 +571,7 @@ const static unsigned short level_mask_bits[] = { #define llama_width 240 #define llama_height 260 -const static unsigned short llama_bits[] = { +static const unsigned short llama_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, @@ -1009,7 +1009,7 @@ const static unsigned short llama_bits[] = { #define llama_mask_width 240 #define llama_mask_height 260 -const static unsigned short llama_mask_bits[] = { +static const unsigned short llama_mask_bits[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, diff --git a/flight/targets/boards/osd/firmware/osd.c b/flight/targets/boards/osd/firmware/osd.c index f43bacc96..499d1d8ca 100644 --- a/flight/targets/boards/osd/firmware/osd.c +++ b/flight/targets/boards/osd/firmware/osd.c @@ -222,7 +222,7 @@ int main() * Runs board and module initialisation, then terminates. */ void -initTask(void *parameters) +initTask(__attribute__((unused)) void *parameters) { /* board driver init */ PIOS_Board_Init(); diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index 1edd83dd8..93db87174 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -128,7 +128,7 @@ static const TIM_TimeBaseInitTypeDef tim_4_time_base = { .TIM_RepetitionCounter = 0x0000, }; -const static struct pios_tim_clock_cfg pios_tim4_cfg = { +static const struct pios_tim_clock_cfg pios_tim4_cfg = { .timer = TIM4, .time_base_init = &tim_4_time_base, .irq = { @@ -469,7 +469,7 @@ PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driv uint16_t supv_timer=0; -static void Clock(uint32_t spektrum_id) { +static void Clock(__attribute__((unused)) uint32_t spektrum_id) { /* 125hz */ ++supv_timer; if(supv_timer >= 625) { diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 4ac4d8062..4ea6bbae8 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -41,6 +41,68 @@ static const struct pios_led pios_leds[] = { }, }, }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif }; static const struct pios_led_cfg pios_led_cfg = { @@ -73,7 +135,7 @@ static const struct pios_led pios_leds_v2[] = { }, }, }, -#ifdef PIOS_RFM22B_DEBUG_ON_SERVO +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM [PIOS_LED_D1] = { .pin = { .gpio = GPIOB, diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index f750288c0..57c9f180e 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -68,6 +68,8 @@ UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += sonaraltitude diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 41baaf0d9..c1acafb7d 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -248,7 +248,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); PIOS_Assert(rx_buffer); - if(tx_buf_len!= -1){ // this is the case for rx/tx ports + if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); PIOS_Assert(tx_buffer); @@ -830,9 +830,11 @@ void PIOS_Board_Init(void) { * \param[in] vcp_port The USB virtual com port options * \param[in] com_speed The com port speed */ -static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, - uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) +static void configureComCallback(__attribute__((unused)) OPLinkSettingsRemoteMainPortOptions main_port, + __attribute__((unused)) OPLinkSettingsRemoteFlexiPortOptions flexi_port, + __attribute__((unused)) OPLinkSettingsRemoteVCPPortOptions vcp_port, + OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { uint32_t comBaud = 9600; switch (com_speed) { diff --git a/flight/targets/boards/revolution/firmware/revolution.c b/flight/targets/boards/revolution/firmware/revolution.c index 65a7db7dd..02c7f0d01 100644 --- a/flight/targets/boards/revolution/firmware/revolution.c +++ b/flight/targets/boards/revolution/firmware/revolution.c @@ -121,7 +121,7 @@ int main() * Runs board and module initialisation, then terminates. */ void -initTask(void *parameters) +initTask(__attribute__((unused)) void *parameters) { /* board driver init */ PIOS_Board_Init(); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 3d6688ddb..ef1434754 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -76,7 +76,7 @@ //------------------------ #define PIOS_LED_HEARTBEAT 0 #define PIOS_LED_ALARM 1 -#ifdef PIOS_RFM22B_DEBUG_ON_SERVO +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM #define PIOS_LED_D1 2 #define PIOS_LED_D2 3 #define PIOS_LED_D3 4 @@ -97,7 +97,7 @@ #define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) #define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) #define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) -#endif +#endif /* PIOS_RFM22B_DEBUG_ON_TELEM */ //------------------------ // PIOS_SPI diff --git a/flight/targets/boards/revoproto/board_hw_defs.c b/flight/targets/boards/revoproto/board_hw_defs.c index 493df060a..0934a5a26 100644 --- a/flight/targets/boards/revoproto/board_hw_defs.c +++ b/flight/targets/boards/revoproto/board_hw_defs.c @@ -60,7 +60,7 @@ static const struct pios_led_cfg pios_led_cfg = { .num_leds = NELEMENTS(pios_leds), }; -const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revision) +const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (__attribute__((unused)) uint32_t board_revision) { return &pios_led_cfg; } diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index bdd1f4dd7..e1b27e5b6 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -41,7 +41,7 @@ MODULES += CameraStab MODULES += FirmwareIAP MODULES += PathPlanner MODULES += FixedWingPathFollower -MODULES += OveroSync +#MODULES += OveroSync ## OveroSync disabled until OP292 is fixed MODULES += Telemetry #MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged #MODULES += Battery diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index f025cd9ea..c7e12bdea 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -73,6 +73,8 @@ UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += sonaraltitude diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index c11651a7c..3fda1d1a6 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -318,7 +318,7 @@ static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(rx_buf_len); PIOS_Assert(rx_buffer); - if(tx_buf_len!= -1){ // this is the case for rx/tx ports + if(tx_buf_len!= (size_t)-1){ // this is the case for rx/tx ports uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(tx_buf_len); PIOS_Assert(tx_buffer); diff --git a/flight/targets/boards/revoproto/firmware/revolution.c b/flight/targets/boards/revoproto/firmware/revolution.c index 5785cd655..b1f771ddd 100644 --- a/flight/targets/boards/revoproto/firmware/revolution.c +++ b/flight/targets/boards/revoproto/firmware/revolution.c @@ -120,7 +120,7 @@ int main() * Runs board and module initialisation, then terminates. */ void -initTask(void *parameters) +initTask(__attribute__((unused)) void *parameters) { /* board driver init */ PIOS_Board_Init(); diff --git a/flight/targets/boards/simposix/firmware/simposix.c b/flight/targets/boards/simposix/firmware/simposix.c index e232259fd..4b6620b9f 100644 --- a/flight/targets/boards/simposix/firmware/simposix.c +++ b/flight/targets/boards/simposix/firmware/simposix.c @@ -121,7 +121,7 @@ int main() * Runs board and module initialisation, then terminates. */ void -initTask(void *parameters) +initTask(__attribute__((unused)) void *parameters) { /* board driver init */ PIOS_Board_Init(); diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index e0ffbc99a..bcf706b78 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -276,8 +276,8 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue */ static void eventTask() { - int32_t timeToNextUpdateMs; - int32_t delayMs; + uint32_t timeToNextUpdateMs; + uint32_t delayMs; EventCallbackInfo evInfo; /* Must do this in task context to ensure that TaskMonitor has already finished its init */ @@ -290,11 +290,7 @@ static void eventTask() while (1) { // Calculate delay time - delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS); - if (delayMs < 0) - { - delayMs = 0; - } + delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS); // Wait for queue message if ( xQueueReceive(queue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE ) diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 16f724d87..d4a22ce10 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -788,7 +788,7 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, * @param[in] file File to append to * @return 0 if success or -1 if failure */ -int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId) +int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { PIOS_Assert(obj_handle); @@ -942,7 +942,7 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) * @param[in] instId The object instance * @return 0 if success or -1 if failure */ -int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId) +int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { PIOS_Assert(obj_handle); @@ -1018,7 +1018,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId) * @param[in] instId The object instance * @return 0 if success or -1 if failure */ -int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId) +int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId) { PIOS_Assert(obj_handle); #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index ba107e970..84174e90a 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -67,7 +67,7 @@ typedef struct { uint8_t timestampLength; uint8_t cs; uint16_t timestamp; - int32_t rxCount; + uint32_t rxCount; UAVTalkRxState state; uint16_t rxPacketLength; } UAVTalkInputProcessor; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 1f25cb767..eb060b735 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -714,7 +714,12 @@ int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_ * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) +static int32_t receiveObject(UAVTalkConnectionData *connection, + uint8_t type, + uint32_t objId, + uint16_t instId, + uint8_t* data, + __attribute__((unused))int32_t length) { UAVObjHandle obj; int32_t ret = 0; diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 38c297f1d..c346f5f97 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -212,6 +213,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // will be dealing with some null pointers addUAVObject("RevoCalibration"); + addUAVObject("EKFConfiguration"); autoLoadWidgets(); // Connect the signals @@ -367,9 +369,9 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y); revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += ( listMean(accel_accum_z) + GRAVITY ); - revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = listMean(gyro_accum_x); - revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = listMean(gyro_accum_y); - revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = listMean(gyro_accum_z); + revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] += listMean(gyro_accum_x); + revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] += listMean(gyro_accum_y); + revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] += listMean(gyro_accum_z); revoCalibration->setData(revoCalibrationData); revoCalibration->updated(); @@ -965,20 +967,20 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj) disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*))); disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*))); - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - Q_ASSERT(revoCalibration); - if(revoCalibration) { - RevoCalibration::DataFields revoCalData = revoCalibration->getData(); - revoCalData.accel_var[RevoCalibration::ACCEL_VAR_X] = listVar(accel_accum_x); - revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Y] = listVar(accel_accum_y); - revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Z] = listVar(accel_accum_z); - revoCalData.gyro_var[RevoCalibration::GYRO_VAR_X] = listVar(gyro_accum_x); - revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Y] = listVar(gyro_accum_y); - revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Z] = listVar(gyro_accum_z); - revoCalData.mag_var[RevoCalibration::MAG_VAR_X] = listVar(mag_accum_x); - revoCalData.mag_var[RevoCalibration::MAG_VAR_Y] = listVar(mag_accum_y); - revoCalData.mag_var[RevoCalibration::MAG_VAR_Z] = listVar(mag_accum_z); - revoCalibration->setData(revoCalData); + EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager()); + Q_ASSERT(ekfConfiguration); + if(ekfConfiguration) { + EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData(); + revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x); + revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y); + revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z); + revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x); + revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y); + revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z); + revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x); + revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y); + revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z); + ekfConfiguration->setData(revoCalData); } } } @@ -989,42 +991,42 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj) */ void ConfigRevoWidget::drawVariancesGraph() { - RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - Q_ASSERT(revoCalibration); - if(!revoCalibration) + EKFConfiguration * ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager()); + Q_ASSERT(ekfConfiguration); + if(!ekfConfiguration) return; - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData(); // The expected range is from 1E-6 to 1E-1 double steps = 6; // 6 bars on the graph - float accel_x_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_X])); + float accel_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX])); if(accel_x) accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); - float accel_y_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Y])); + float accel_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY])); if(accel_y) accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); - float accel_z_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Z])); + float accel_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ])); if(accel_z) accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); - float gyro_x_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_X])); + float gyro_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX])); if(gyro_x) gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); - float gyro_y_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Y])); + float gyro_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY])); if(gyro_y) gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); - float gyro_z_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Z])); + float gyro_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ])); if(gyro_z) gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); // Scale by 1e-3 because mag vars are much higher. - float mag_x_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_X])); + float mag_x_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGX])); if(mag_x) mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); - float mag_y_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Y])); + float mag_y_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGY])); if(mag_y) mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); - float mag_z_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Z])); + float mag_z_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGZ])); if(mag_z) mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); } diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 54447b84b..b8cea0d8c 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -75,8 +75,6 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) // control_sock->close(); control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress); - - controlsMode = GCSControlConfig->getControlsMode(); int i; @@ -102,6 +100,20 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { double pitch = obj->getField("Pitch")->getDouble(); double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); + + // necessary against having the wrong joystick profile chosen, which shows weird values + if (throttle > -1.0 && throttle <= 1.0) { + // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) + throttle = -1.0 + (throttle * 2.0); + } else { + // with the safety value (line 206), this helps keep the sticks insde the margins + if (throttle <= -1.0) { + throttle = -1.0; + } else { + throttle = 1.0; + } + } + // Remap RPYT to left X/Y and right X/Y depending on mode switch (controlsMode) { case 1: @@ -187,6 +199,17 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) return; + //if (newThrottle != oldThrottle) { + // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + newThrottle = (newThrottle + 1.0) / 2.0; + + // safety value to stop the motors from spinning at 0% throttle + if (newThrottle <= 0.01 ) { + newThrottle = -1; + } + //} + + if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); if (buttonPitchControl==0)obj->getField("Pitch")->setDouble(newPitch); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 82a5f7b73..bbafda25e 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -70,9 +70,6 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) // Connect object updated event from UAVObject to also update check boxes and dropdown connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); - - - leftX = 0; leftY = 0; rightX = 0; @@ -129,7 +126,6 @@ void GCSControlGadgetWidget::toggleControl(int state) UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; m_gcscontrol->checkBoxUDPControl->setEnabled(true); - } else { diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 42536c390..43a772af5 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -33,6 +33,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/ekfconfiguration.h \ + $$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \ $$UAVOBJECT_SYNTHETICS/revosettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ @@ -114,6 +116,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/ekfconfiguration.cpp \ + $$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ $$UAVOBJECT_SYNTHETICS/revosettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 696d9dfd3..18b97a225 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -211,9 +211,9 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString(" data.%1 = %2;\n") - .arg( info->fields[n]->name ) - .arg( info->fields[n]->defaultValues[0].toFloat() ) ); + initfields.append( QString(" data.%1 = %2f;\n") + .arg(info->fields[n]->name) + .arg(info->fields[n]->defaultValues[0].toFloat(), 0, 'e', 6)); } else { @@ -236,10 +236,10 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) } else if ( info->fields[n]->type == FIELDTYPE_FLOAT32 ) { - initfields.append( QString(" data.%1[%2] = %3;\n") - .arg( info->fields[n]->name ) - .arg( idx ) - .arg( info->fields[n]->defaultValues[idx].toFloat() ) ); + initfields.append( QString(" data.%1[%2] = %3f;\n") + .arg(info->fields[n]->name) + .arg(idx) + .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); } else { diff --git a/make/common-defs.mk b/make/common-defs.mk index 67fd3074a..f73624c13 100644 --- a/make/common-defs.mk +++ b/make/common-defs.mk @@ -123,15 +123,12 @@ CFLAGS += -O$(OPT) CFLAGS += -g$(DEBUGF) CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer -CFLAGS += -Wall +CFLAGS += -Wall -Wextra +CFLAGS += -Wfloat-equal -Wunsuffixed-float-constants -Wdouble-promotion +CFLAGS += -Werror CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) -# FIXME: stm32f4xx library raises strict aliasing and const qualifier warnings -ifneq ($(MCU),cortex-m4) - CFLAGS += -Werror -endif - ifeq ($(DEBUG), YES) CFLAGS += -DDEBUG else diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml new file mode 100644 index 000000000..73fafdfdc --- /dev/null +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -0,0 +1,74 @@ + + + Extended Kalman Filter initialisation + + + PositionNorth + PositionEast + PositionDown + VelocityNorth + VelocityEast + VelocityDown + AttitudeQ1 + AttitudeQ2 + AttitudeQ3 + AttitudeQ4 + GyroDriftX + GyroDriftY + GyroDriftZ + + + + + GyroX + GyroY + GyroZ + AccelX + AccelY + AccelZ + GyroDriftX + GyroDriftY + GyroDriftZ + + + + + GPSPosNorth + GPSPosEast + GPSPosDown + GPSVelNorth + GPSVelEast + GPSVelDown + MagX + MagY + MagZ + BaroZ + + + + + FakeGPSPosIndoor + FakeGPSVelIndoor + FakeGPSVelAirspeed + + + + + + + + diff --git a/shared/uavobjectdefinition/ekfstatevariance.xml b/shared/uavobjectdefinition/ekfstatevariance.xml new file mode 100644 index 000000000..05b684dbd --- /dev/null +++ b/shared/uavobjectdefinition/ekfstatevariance.xml @@ -0,0 +1,26 @@ + + + Extended Kalman Filter state covariance + + + PositionNorth + PositionEast + PositionDown + VelocityNorth + VelocityEast + VelocityDown + AttitudeQ1 + AttitudeQ2 + AttitudeQ3 + AttitudeQ4 + GyroDriftX + GyroDriftY + GyroDriftZ + + + + + + + + diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml index d1875224c..ed620769e 100644 --- a/shared/uavobjectdefinition/revocalibration.xml +++ b/shared/uavobjectdefinition/revocalibration.xml @@ -3,22 +3,16 @@ Settings for the INS to control the algorithm and what is updated - + - - - - - - - + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index e4e3a402a..9fc3681b1 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -27,7 +27,7 @@ - +