mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
Merge branch 'amorale/OP-908_clean_float_math' into next
Conflicts: flight/PiOS/Common/pios_rfm22b.c
This commit is contained in:
Alessio Morale
commit
2fe7139fc3
@ -29,11 +29,10 @@
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <pios_math.h>
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define RAD2DEG (180.0f/ F_PI)
|
||||
#define DEG2RAD (F_PI /180.0f)
|
||||
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
|
||||
|
||||
// ****** convert Lat,Lon,Alt to ECEF ************
|
||||
void LLA2ECEF(float LLA[3], float ECEF[3])
|
||||
@ -43,10 +42,10 @@ void LLA2ECEF(float LLA[3], float ECEF[3])
|
||||
float sinLat, sinLon, cosLat, cosLon;
|
||||
float N;
|
||||
|
||||
sinLat = sinf(DEG2RAD * LLA[0]);
|
||||
sinLon = sinf(DEG2RAD * LLA[1]);
|
||||
cosLat = cosf(DEG2RAD * LLA[0]);
|
||||
cosLon = cosf(DEG2RAD * LLA[1]);
|
||||
sinLat = sinf(DEG2RAD(LLA[0]));
|
||||
sinLon = sinf(DEG2RAD(LLA[1]));
|
||||
cosLat = cosf(DEG2RAD(LLA[0]));
|
||||
cosLon = cosf(DEG2RAD(LLA[1]));
|
||||
|
||||
N = a / sqrtf(1.0f - e * e * sinLat * sinLat); //prime vertical radius of curvature
|
||||
|
||||
@ -73,10 +72,10 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
|
||||
float Lat, N, NplusH, delta, esLat;
|
||||
uint16_t iter;
|
||||
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
|
||||
#define ACCURACY 1.0e-11 // used to be e-14, but we don't need sub micrometer exact calculations
|
||||
#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
|
||||
|
||||
LLA[1] = RAD2DEG * atan2f(y, x);
|
||||
Lat = DEG2RAD * LLA[0];
|
||||
LLA[1] = RAD2DEG(atan2f(y, x));
|
||||
Lat = DEG2RAD(LLA[0]);
|
||||
esLat = e * sinf(Lat);
|
||||
N = a / sqrtf(1 - esLat * esLat);
|
||||
NplusH = N + LLA[2];
|
||||
@ -93,7 +92,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
|
||||
iter += 1;
|
||||
}
|
||||
|
||||
LLA[0] = RAD2DEG * Lat;
|
||||
LLA[0] = RAD2DEG(Lat);
|
||||
LLA[2] = NplusH - N;
|
||||
|
||||
return (iter < MAX_ITER);
|
||||
@ -104,10 +103,10 @@ void RneFromLLA(float LLA[3], float Rne[3][3])
|
||||
{
|
||||
float sinLat, sinLon, cosLat, cosLon;
|
||||
|
||||
sinLat = (float)sinf(DEG2RAD * LLA[0]);
|
||||
sinLon = (float)sinf(DEG2RAD * LLA[1]);
|
||||
cosLat = (float)cosf(DEG2RAD * LLA[0]);
|
||||
cosLon = (float)cosf(DEG2RAD * LLA[1]);
|
||||
sinLat = (float)sinf(DEG2RAD(LLA[0]));
|
||||
sinLon = (float)sinf(DEG2RAD(LLA[1]));
|
||||
cosLat = (float)cosf(DEG2RAD(LLA[0]));
|
||||
cosLon = (float)cosf(DEG2RAD(LLA[1]));
|
||||
|
||||
Rne[0][0] = -sinLat * cosLon;
|
||||
Rne[0][1] = -sinLat * sinLon;
|
||||
@ -135,9 +134,9 @@ void Quaternion2RPY(const float q[4], float rpy[3])
|
||||
R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
|
||||
R33 = q0s - q1s - q2s + q3s;
|
||||
|
||||
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
|
||||
rpy[2] = RAD2DEG * atan2f(R12, R11);
|
||||
rpy[0] = RAD2DEG * atan2f(R23, R33);
|
||||
rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2
|
||||
rpy[2] = RAD2DEG(atan2f(R12, R11));
|
||||
rpy[0] = RAD2DEG(atan2f(R23, R33));
|
||||
|
||||
//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
|
||||
}
|
||||
@ -148,9 +147,9 @@ void RPY2Quaternion(const float rpy[3], float q[4])
|
||||
float phi, theta, psi;
|
||||
float cphi, sphi, ctheta, stheta, cpsi, spsi;
|
||||
|
||||
phi = DEG2RAD * rpy[0] / 2;
|
||||
theta = DEG2RAD * rpy[1] / 2;
|
||||
psi = DEG2RAD * rpy[2] / 2;
|
||||
phi = DEG2RAD(rpy[0] / 2);
|
||||
theta = DEG2RAD(rpy[1] / 2);
|
||||
psi = DEG2RAD(rpy[2] / 2);
|
||||
cphi = cosf(phi);
|
||||
sphi = sinf(phi);
|
||||
ctheta = cosf(theta);
|
||||
@ -294,13 +293,13 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
|
||||
|
||||
// The first rows of rot matrices chosen in direction of v1
|
||||
mag = VectorMagnitude(v1b);
|
||||
if (fabs(mag) < 1e-30)
|
||||
if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
|
||||
return (-1);
|
||||
for (i=0;i<3;i++)
|
||||
Rib[0][i]=v1b[i]/mag;
|
||||
|
||||
mag = VectorMagnitude(v1e);
|
||||
if (fabs(mag) < 1e-30)
|
||||
if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
|
||||
return (-1);
|
||||
for (i=0;i<3;i++)
|
||||
Rie[0][i]=v1e[i]/mag;
|
||||
@ -308,14 +307,14 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
|
||||
// The second rows of rot matrices chosen in direction of v1xv2
|
||||
CrossProduct(v1b,v2b,&Rib[1][0]);
|
||||
mag = VectorMagnitude(&Rib[1][0]);
|
||||
if (fabs(mag) < 1e-30)
|
||||
if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
|
||||
return (-1);
|
||||
for (i=0;i<3;i++)
|
||||
Rib[1][i]=Rib[1][i]/mag;
|
||||
|
||||
CrossProduct(v1e,v2e,&Rie[1][0]);
|
||||
mag = VectorMagnitude(&Rie[1][0]);
|
||||
if (fabs(mag) < 1e-30)
|
||||
if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
|
||||
return (-1);
|
||||
for (i=0;i<3;i++)
|
||||
Rie[1][i]=Rie[1][i]/mag;
|
||||
|
||||
@ -57,7 +57,8 @@
|
||||
//#define MALLOC(x) malloc(x)
|
||||
//#define FREE(x) free(x)
|
||||
|
||||
// const should hopefully keep them in the flash region
|
||||
// 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},
|
||||
@ -204,11 +205,11 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
|
||||
// ***********
|
||||
// range check supplied params
|
||||
|
||||
if (Lat < -90) return -1; // error
|
||||
if (Lat > 90) return -2; // error
|
||||
if (Lat < -90.0f) return -1; // error
|
||||
if (Lat > 90.0f) return -2; // error
|
||||
|
||||
if (Lon < -180) return -3; // error
|
||||
if (Lon > 180) return -4; // error
|
||||
if (Lon < -180.0f) return -3; // error
|
||||
if (Lon > 180.0f) return -4; // error
|
||||
|
||||
// ***********
|
||||
// allocated required memory
|
||||
@ -242,7 +243,7 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
|
||||
{
|
||||
CoordGeodetic->lambda = Lon;
|
||||
CoordGeodetic->phi = Lat;
|
||||
CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0; // convert to km
|
||||
CoordGeodetic->HeightAboveEllipsoid = AltEllipsoid/1000.0f; // convert to km
|
||||
|
||||
// Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report
|
||||
if (WMM_GeodeticToSpherical(CoordGeodetic, CoordSpherical) < 0)
|
||||
@ -293,9 +294,9 @@ int WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, u
|
||||
Ellip = NULL;
|
||||
}
|
||||
|
||||
B[0] = GeoMagneticElements->X * 1e-2;
|
||||
B[1] = GeoMagneticElements->Y * 1e-2;
|
||||
B[2] = GeoMagneticElements->Z * 1e-2;
|
||||
B[0] = GeoMagneticElements->X * 1e-2f;
|
||||
B[1] = GeoMagneticElements->Y * 1e-2f;
|
||||
B[2] = GeoMagneticElements->Z * 1e-2f;
|
||||
|
||||
return returned;
|
||||
}
|
||||
@ -433,8 +434,8 @@ int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical
|
||||
float cos_lambda, sin_lambda;
|
||||
uint16_t m, n;
|
||||
|
||||
cos_lambda = cos(DEG2RAD(CoordSpherical->lambda));
|
||||
sin_lambda = sin(DEG2RAD(CoordSpherical->lambda));
|
||||
cos_lambda = cosf(DEG2RAD(CoordSpherical->lambda));
|
||||
sin_lambda = sinf(DEG2RAD(CoordSpherical->lambda));
|
||||
|
||||
/* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2)
|
||||
for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */
|
||||
@ -444,12 +445,12 @@ int WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSpherical
|
||||
SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n - 1] * (Ellip->re / CoordSpherical->r);
|
||||
|
||||
/*
|
||||
Compute cos(m*lambda), sin(m*lambda) for m = 0 ... nMax
|
||||
cos(a + b) = cos(a)*cos(b) - sin(a)*sin(b)
|
||||
sin(a + b) = cos(a)*sin(b) + sin(a)*cos(b)
|
||||
Compute cosf(m*lambda), sinf(m*lambda) for m = 0 ... nMax
|
||||
cosf(a + b) = cosf(a)*cosf(b) - sinf(a)*sinf(b)
|
||||
sinf(a + b) = cosf(a)*sinf(b) + sinf(a)*cosf(b)
|
||||
*/
|
||||
SphVariables->cos_mlambda[0] = 1.0;
|
||||
SphVariables->sin_mlambda[0] = 0.0;
|
||||
SphVariables->cos_mlambda[0] = 1.0f;
|
||||
SphVariables->sin_mlambda[0] = 0.0f;
|
||||
|
||||
SphVariables->cos_mlambda[1] = cos_lambda;
|
||||
SphVariables->sin_mlambda[1] = sin_lambda;
|
||||
@ -480,9 +481,9 @@ int WMM_AssociatedLegendreFunction(WMMtype_CoordSpherical * CoordSpherical, uint
|
||||
|
||||
*/
|
||||
{
|
||||
float sin_phi = sin(DEG2RAD(CoordSpherical->phig)); /* sin (geocentric latitude) */
|
||||
float sin_phi = sinf(DEG2RAD(CoordSpherical->phig)); /* sinf (geocentric latitude) */
|
||||
|
||||
if (nMax <= 16 || (1 - fabs(sin_phi)) < 1.0e-10) /* If nMax is less tha 16 or at the poles */
|
||||
if (nMax <= 16 || (1 - fabsf(sin_phi)) < 1.0e-10f) /* If nMax is less tha 16 or at the poles */
|
||||
{
|
||||
if (WMM_PcupLow(LegendreFunction->Pcup, LegendreFunction->dPcup, sin_phi, nMax) < 0)
|
||||
return -1; // error
|
||||
@ -508,7 +509,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
|
||||
dV ^ 1 dV ^ 1 dV ^
|
||||
grad V = -- r + - -- t + -------- -- p
|
||||
dr r dt r sin(t) dp
|
||||
dr r dt r sinf(t) dp
|
||||
|
||||
INPUT : LegendreFunction
|
||||
MagneticModel
|
||||
@ -535,7 +536,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
index = (n * (n + 1) / 2 + m);
|
||||
|
||||
/* nMax (n+2) n m m m
|
||||
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi))
|
||||
Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/
|
||||
MagneticResults->Bz -=
|
||||
@ -545,7 +546,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
* (float)(n + 1) * LegendreFunction->Pcup[index];
|
||||
|
||||
/* 1 nMax (n+2) n m m m
|
||||
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
|
||||
MagneticResults->By +=
|
||||
@ -554,7 +555,7 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
SphVariables->sin_mlambda[m] - WMM_get_main_field_coeff_h(index) * SphVariables->cos_mlambda[m])
|
||||
* (float)(m) * LegendreFunction->Pcup[index];
|
||||
/* nMax (n+2) n m m m
|
||||
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */
|
||||
|
||||
@ -567,8 +568,8 @@ int WMM_Summation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
}
|
||||
}
|
||||
|
||||
cos_phi = cos(DEG2RAD(CoordSpherical->phig));
|
||||
if (fabs(cos_phi) > 1.0e-10)
|
||||
cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
|
||||
if (fabsf(cos_phi) > 1.0e-10f)
|
||||
{
|
||||
MagneticResults->By = MagneticResults->By / cos_phi;
|
||||
}
|
||||
@ -617,7 +618,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
index = (n * (n + 1) / 2 + m);
|
||||
|
||||
/* nMax (n+2) n m m m
|
||||
Bz = -SUM (a/r) (n+1) SUM [g cos(m p) + h sin(m p)] P (sin(phi))
|
||||
Bz = -SUM (a/r) (n+1) SUM [g cosf(m p) + h sinf(m p)] P (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Derivative with respect to radius.*/
|
||||
MagneticResults->Bz -=
|
||||
@ -627,7 +628,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
* (float)(n + 1) * LegendreFunction->Pcup[index];
|
||||
|
||||
/* 1 nMax (n+2) n m m m
|
||||
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Derivative with respect to longitude, divided by radius. */
|
||||
MagneticResults->By +=
|
||||
@ -636,7 +637,7 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
SphVariables->sin_mlambda[m] - WMM_get_secular_var_coeff_h(index) * SphVariables->cos_mlambda[m])
|
||||
* (float)(m) * LegendreFunction->Pcup[index];
|
||||
/* nMax (n+2) n m m m
|
||||
Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
Bx = - SUM (a/r) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Derivative with respect to latitude, divided by radius. */
|
||||
|
||||
@ -647,8 +648,8 @@ int WMM_SecVarSummation(WMMtype_LegendreFunction * LegendreFunction,
|
||||
* LegendreFunction->dPcup[index];
|
||||
}
|
||||
}
|
||||
cos_phi = cos(DEG2RAD(CoordSpherical->phig));
|
||||
if (fabs(cos_phi) > 1.0e-10)
|
||||
cos_phi = cosf(DEG2RAD(CoordSpherical->phig));
|
||||
if (fabsf(cos_phi) > 1.0e-10f)
|
||||
{
|
||||
MagneticResults->By = MagneticResults->By / cos_phi;
|
||||
}
|
||||
@ -695,11 +696,11 @@ int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
|
||||
*/
|
||||
{
|
||||
/* Difference between the spherical and Geodetic latitudes */
|
||||
float Psi = (M_PI / 180) * (CoordSpherical->phig - CoordGeodetic->phi);
|
||||
float Psi = (M_PI_F / 180) * (CoordSpherical->phig - CoordGeodetic->phi);
|
||||
|
||||
/* Rotate spherical field components to the Geodeitic system */
|
||||
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sin(Psi) + MagneticResultsSph->Bz * cos(Psi);
|
||||
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cos(Psi) - MagneticResultsSph->Bz * sin(Psi);
|
||||
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi);
|
||||
MagneticResultsGeo->Bx = MagneticResultsSph->Bx * cosf(Psi) - MagneticResultsSph->Bz * sinf(Psi);
|
||||
MagneticResultsGeo->By = MagneticResultsSph->By;
|
||||
|
||||
return 0;
|
||||
@ -727,10 +728,10 @@ int WMM_CalculateGeoMagneticElements(WMMtype_MagneticResults * MagneticResultsGe
|
||||
GeoMagneticElements->Y = MagneticResultsGeo->By;
|
||||
GeoMagneticElements->Z = MagneticResultsGeo->Bz;
|
||||
|
||||
GeoMagneticElements->H = sqrt(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By);
|
||||
GeoMagneticElements->F = sqrt(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
|
||||
GeoMagneticElements->Decl = RAD2DEG(atan2(GeoMagneticElements->Y, GeoMagneticElements->X));
|
||||
GeoMagneticElements->Incl = RAD2DEG(atan2(GeoMagneticElements->Z, GeoMagneticElements->H));
|
||||
GeoMagneticElements->H = sqrtf(MagneticResultsGeo->Bx * MagneticResultsGeo->Bx + MagneticResultsGeo->By * MagneticResultsGeo->By);
|
||||
GeoMagneticElements->F = sqrtf(GeoMagneticElements->H * GeoMagneticElements->H + MagneticResultsGeo->Bz * MagneticResultsGeo->Bz);
|
||||
GeoMagneticElements->Decl = RAD2DEG(atan2f(GeoMagneticElements->Y, GeoMagneticElements->X));
|
||||
GeoMagneticElements->Incl = RAD2DEG(atan2f(GeoMagneticElements->Z, GeoMagneticElements->H));
|
||||
|
||||
return 0; // OK
|
||||
}
|
||||
@ -762,10 +763,10 @@ int WMM_CalculateSecularVariation(WMMtype_MagneticResults * MagneticVariation, W
|
||||
(MagneticElements->X * MagneticElements->Xdot +
|
||||
MagneticElements->Y * MagneticElements->Ydot + MagneticElements->Z * MagneticElements->Zdot) / MagneticElements->F;
|
||||
MagneticElements->Decldot =
|
||||
180.0 / M_PI * (MagneticElements->X * MagneticElements->Ydot -
|
||||
180.0f / M_PI_F * (MagneticElements->X * MagneticElements->Ydot -
|
||||
MagneticElements->Y * MagneticElements->Xdot) / (MagneticElements->H * MagneticElements->H);
|
||||
MagneticElements->Incldot =
|
||||
180.0 / M_PI * (MagneticElements->H * MagneticElements->Zdot -
|
||||
180.0f / M_PI_F * (MagneticElements->H * MagneticElements->Zdot -
|
||||
MagneticElements->Z * MagneticElements->Hdot) / (MagneticElements->F * MagneticElements->F);
|
||||
MagneticElements->GVdot = MagneticElements->Decldot;
|
||||
|
||||
@ -776,7 +777,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
|
||||
/* This function evaluates all of the Schmidt-semi normalized associated Legendre
|
||||
functions up to degree nMax. The functions are initially scaled by
|
||||
10^280 sin^m in order to minimize the effects of underflow at large m
|
||||
10^280 sinf^m in order to minimize the effects of underflow at large m
|
||||
near the poles (see Holmes and Featherstone 2002, J. Geodesy, 76, 279-299).
|
||||
Note that this function performs the same operation as WMM_PcupLow.
|
||||
However this function also can be used for high degree (large nMax) models.
|
||||
@ -784,7 +785,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
Calling Parameters:
|
||||
INPUT
|
||||
nMax: Maximum spherical harmonic degree to compute.
|
||||
x: cos(colatitude) or sin(latitude).
|
||||
x: cosf(colatitude) or sinf(latitude).
|
||||
|
||||
OUTPUT
|
||||
Pcup: A vector of all associated Legendgre polynomials evaluated at
|
||||
@ -800,9 +801,9 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
|
||||
Change from the previous version
|
||||
The prevous version computes the derivatives as
|
||||
dP(n,m)(x)/dx, where x = sin(latitude) (or cos(colatitude) ).
|
||||
dP(n,m)(x)/dx, where x = sinf(latitude) (or cosf(colatitude) ).
|
||||
However, the WMM Geomagnetic routines requires dP(n,m)(x)/dlatitude.
|
||||
Hence the derivatives are multiplied by sin(latitude).
|
||||
Hence the derivatives are multiplied by sinf(latitude).
|
||||
Removed the options for CS phase and normalizations.
|
||||
|
||||
Note: In geomagnetism, the derivatives of ALF are usually found with
|
||||
@ -842,7 +843,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
scalef = 1.0e-280;
|
||||
|
||||
for (n = 0; n <= 2 * nMax + 1; ++n)
|
||||
PreSqr[n] = sqrt((float)(n));
|
||||
PreSqr[n] = sqrtf((float)(n));
|
||||
|
||||
k = 2;
|
||||
|
||||
@ -860,10 +861,10 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
k = k + 2;
|
||||
}
|
||||
|
||||
/*z = sin (geocentric latitude) */
|
||||
z = sqrt((1.0 - x) * (1.0 + x));
|
||||
/*z = sinf (geocentric latitude) */
|
||||
z = sqrtf((1.0f - x) * (1.0f + x));
|
||||
pm2 = 1.0;
|
||||
Pcup[0] = 1.0;
|
||||
Pcup[0] = 1.0f;
|
||||
dPcup[0] = 0.0;
|
||||
if (nMax == 0)
|
||||
{
|
||||
@ -945,7 +946,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
Calling Parameters:
|
||||
INPUT
|
||||
nMax: Maximum spherical harmonic degree to compute.
|
||||
x: cos(colatitude) or sin(latitude).
|
||||
x: cosf(colatitude) or sinf(latitude).
|
||||
|
||||
OUTPUT
|
||||
Pcup: A vector of all associated Legendgre polynomials evaluated at
|
||||
@ -975,8 +976,8 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
Pcup[0] = 1.0;
|
||||
dPcup[0] = 0.0;
|
||||
|
||||
/*sin (geocentric latitude) - sin_phi */
|
||||
z = sqrt((1.0 - x) * (1.0 + x));
|
||||
/*sinf (geocentric latitude) - sin_phi */
|
||||
z = sqrtf((1.0f - x) * (1.0f + x));
|
||||
|
||||
/* First, Compute the Gauss-normalized associated Legendre functions */
|
||||
for (n = 1; n <= nMax; n++)
|
||||
@ -1033,7 +1034,7 @@ int WMM_PcupLow(float *Pcup, float *dPcup, float x, uint16_t nMax)
|
||||
{
|
||||
index = (n * (n + 1) / 2 + m);
|
||||
index1 = (n * (n + 1) / 2 + m - 1);
|
||||
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrt((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m));
|
||||
schmidtQuasiNorm[index] = schmidtQuasiNorm[index1] * sqrtf((float)((n - m + 1) * (m == 1 ? 2 : 1)) / (float)(n + m));
|
||||
}
|
||||
|
||||
}
|
||||
@ -1086,7 +1087,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
schmidtQuasiNorm1 = 1.0;
|
||||
|
||||
MagneticResults->By = 0.0;
|
||||
sin_phi = sin(DEG2RAD(CoordSpherical->phig));
|
||||
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
|
||||
|
||||
for (n = 1; n <= MagneticModel->nMax; n++)
|
||||
{
|
||||
@ -1097,7 +1098,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
|
||||
index = (n * (n + 1) / 2 + 1);
|
||||
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
|
||||
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1));
|
||||
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1));
|
||||
schmidtQuasiNorm1 = schmidtQuasiNorm2;
|
||||
if (n == 1)
|
||||
{
|
||||
@ -1110,7 +1111,7 @@ int WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
}
|
||||
|
||||
/* 1 nMax (n+2) n m m m
|
||||
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */
|
||||
|
||||
@ -1152,13 +1153,13 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
schmidtQuasiNorm1 = 1.0;
|
||||
|
||||
MagneticResults->By = 0.0;
|
||||
sin_phi = sin(DEG2RAD(CoordSpherical->phig));
|
||||
sin_phi = sinf(DEG2RAD(CoordSpherical->phig));
|
||||
|
||||
for (n = 1; n <= MagneticModel->nMaxSecVar; n++)
|
||||
{
|
||||
index = (n * (n + 1) / 2 + 1);
|
||||
schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float)(2 * n - 1) / (float)n;
|
||||
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrt((float)(n * 2) / (float)(n + 1));
|
||||
schmidtQuasiNorm3 = schmidtQuasiNorm2 * sqrtf((float)(n * 2) / (float)(n + 1));
|
||||
schmidtQuasiNorm1 = schmidtQuasiNorm2;
|
||||
if (n == 1)
|
||||
{
|
||||
@ -1171,7 +1172,7 @@ int WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables *
|
||||
}
|
||||
|
||||
/* 1 nMax (n+2) n m m m
|
||||
By = SUM (a/r) (m) SUM [g cos(m p) + h sin(m p)] dP (sin(phi))
|
||||
By = SUM (a/r) (m) SUM [g cosf(m p) + h sinf(m p)] dP (sinf(phi))
|
||||
n=1 m=0 n n n */
|
||||
/* Derivative with respect to longitude, divided by radius. */
|
||||
|
||||
@ -1291,7 +1292,7 @@ int WMM_DateToYear(uint16_t month, uint16_t day, uint16_t year)
|
||||
temp += MonthDays[i - 1];
|
||||
temp += day;
|
||||
|
||||
decimal_date = year + (temp - 1) / (365.0 + ExtraDay);
|
||||
decimal_date = year + (temp - 1) / (365.0f + ExtraDay);
|
||||
|
||||
return 0; // OK
|
||||
}
|
||||
@ -1304,21 +1305,21 @@ int WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_Coord
|
||||
{
|
||||
float CosLat, SinLat, rc, xp, zp; // all local variables
|
||||
|
||||
CosLat = cos(DEG2RAD(CoordGeodetic->phi));
|
||||
SinLat = sin(DEG2RAD(CoordGeodetic->phi));
|
||||
CosLat = cosf(DEG2RAD(CoordGeodetic->phi));
|
||||
SinLat = sinf(DEG2RAD(CoordGeodetic->phi));
|
||||
|
||||
// compute the local radius of curvature on the WGS-84 reference ellipsoid
|
||||
rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat);
|
||||
rc = Ellip->a / sqrtf(1.0f - Ellip->epssq * SinLat * SinLat);
|
||||
|
||||
// compute ECEF Cartesian coordinates of specified point (for longitude=0)
|
||||
|
||||
xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat;
|
||||
zp = (rc * (1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
|
||||
zp = (rc * (1.0f - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat;
|
||||
|
||||
// compute spherical radius and angle lambda and phi of specified point
|
||||
|
||||
CoordSpherical->r = sqrt(xp * xp + zp * zp);
|
||||
CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude
|
||||
CoordSpherical->r = sqrtf(xp * xp + zp * zp);
|
||||
CoordSpherical->phig = RAD2DEG(asinf(zp / CoordSpherical->r)); // geocentric latitude
|
||||
CoordSpherical->lambda = CoordGeodetic->lambda; // longitude
|
||||
|
||||
return 0; // OK
|
||||
|
||||
@ -26,6 +26,7 @@
|
||||
|
||||
#ifndef WMMINTERNAL_H_
|
||||
#define WMMINTERNAL_H_
|
||||
#include <pios_math.h>
|
||||
|
||||
// internal constants
|
||||
#define TRUE ((uint16_t)1)
|
||||
@ -35,8 +36,6 @@
|
||||
#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2);
|
||||
#define NUMPCUP 92 // NUMTERMS +1
|
||||
#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1
|
||||
#define RAD2DEG(rad) ((rad)*(180.0L/M_PI))
|
||||
#define DEG2RAD(deg) ((deg)*(M_PI/180.0L))
|
||||
|
||||
// internal structure definitions
|
||||
typedef struct {
|
||||
|
||||
@ -32,6 +32,7 @@
|
||||
#include "math.h"
|
||||
#include "stdbool.h"
|
||||
#include "stdint.h"
|
||||
#include <pios_math.h>
|
||||
|
||||
#define FLASH_TABLE
|
||||
#ifdef FLASH_TABLE
|
||||
@ -81,7 +82,7 @@ int sin_lookup_initalize()
|
||||
return -1;
|
||||
|
||||
for(uint32_t i = 0; i < 180; i++)
|
||||
sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f);
|
||||
sin_table[i] = sinf(DEG2RAD((float)i));
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -126,7 +127,7 @@ float cos_lookup_deg(float angle)
|
||||
*/
|
||||
float sin_lookup_rad(float angle)
|
||||
{
|
||||
int degrees = angle * 180.0f / M_PI;
|
||||
int degrees = angle * 180.0f / M_PI_F;
|
||||
return sin_lookup_deg(degrees);
|
||||
}
|
||||
|
||||
@ -137,6 +138,6 @@ float sin_lookup_rad(float angle)
|
||||
*/
|
||||
float cos_lookup_rad(float angle)
|
||||
{
|
||||
int degrees = angle * 180.0f / M_PI;
|
||||
int degrees = angle * 180.0f / M_PI_F;
|
||||
return cos_lookup_deg(degrees);
|
||||
}
|
||||
}
|
||||
|
||||
@ -94,7 +94,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
|
||||
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
|
||||
dist_path = sqrtf( path_north * path_north + path_east * path_east );
|
||||
|
||||
if(dist_diff < 1e-6 ) {
|
||||
if (dist_diff < 1e-6f ) {
|
||||
status->fractional_progress = 1;
|
||||
status->error = 0;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
@ -135,7 +135,7 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
|
||||
dot = path_north * diff_north + path_east * diff_east;
|
||||
dist_path = sqrtf( path_north * path_north + path_east * path_east );
|
||||
|
||||
if(dist_path < 1e-6) {
|
||||
if (dist_path < 1e-6f){
|
||||
// if the path is too short, we cannot determine vector direction.
|
||||
// Fly towards the endpoint to prevent flying away,
|
||||
// but assume progress=1 either way.
|
||||
@ -188,7 +188,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
|
||||
radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
|
||||
cradius = sqrtf( diff_north * diff_north + diff_east * diff_east );
|
||||
|
||||
if (cradius < 1e-6) {
|
||||
if (cradius < 1e-6f) {
|
||||
// cradius is zero, just fly somewhere and make sure correction is still a normal
|
||||
status->fractional_progress = 1;
|
||||
status->error = radius;
|
||||
|
||||
@ -35,7 +35,7 @@
|
||||
#include "gpsvelocity.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#include <pios_math.h>
|
||||
|
||||
|
||||
// Private constants
|
||||
@ -44,9 +44,6 @@
|
||||
#define SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO
|
||||
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO
|
||||
|
||||
#define F_PI 3.141526535897932f
|
||||
#define DEG2RAD (F_PI/180.0f)
|
||||
|
||||
// Private types
|
||||
struct GPSGlobals {
|
||||
float RbeCol1_old[3];
|
||||
@ -117,7 +114,7 @@ void gps_airspeedGet(float *v_air_GPS)
|
||||
float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]);
|
||||
|
||||
//If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
|
||||
if (fabs(cosDiff) < cos(5.0f*DEG2RAD)) {
|
||||
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
|
||||
GPSVelocityData gpsVelData;
|
||||
GPSVelocityGet(&gpsVelData);
|
||||
|
||||
|
||||
@ -58,6 +58,8 @@
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include <pios_board_info.h>
|
||||
#include <pios_math.h>
|
||||
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 540
|
||||
@ -67,7 +69,6 @@
|
||||
#define UPDATE_RATE 25.0f
|
||||
#define GYRO_NEUTRAL 1665
|
||||
|
||||
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
@ -334,7 +335,7 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
|
||||
float throttle;
|
||||
FlightStatusArmedGet(&armed);
|
||||
ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne
|
||||
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
|
||||
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) {
|
||||
trim_samples++;
|
||||
// Store the digitally scaled version since that is what we use for bias
|
||||
trim_accels[0] += accels->x;
|
||||
@ -513,10 +514,10 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
// 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] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f);
|
||||
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f);
|
||||
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f);
|
||||
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f);
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
@ -541,7 +542,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
|
||||
if((fabsf(qmag) < 1e-3f) || (qmag != qmag)) {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
@ -571,7 +572,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
|
||||
|
||||
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
|
||||
const float fakeDt = 0.0025;
|
||||
if (attitudeSettings.AccelTau < 0.0001) {
|
||||
if (attitudeSettings.AccelTau < 0.0001f) {
|
||||
accel_alpha = 0; // not trusting this to resolve to 0
|
||||
accel_filter_enabled = false;
|
||||
} else {
|
||||
|
||||
@ -70,15 +70,12 @@
|
||||
#include "revosettings.h"
|
||||
#include "velocityactual.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#include <pios_math.h>
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
#define FAILSAFE_TIMEOUT_MS 10
|
||||
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
|
||||
|
||||
// low pass filter configuration to calculate offset
|
||||
// of barometric altitude sensor
|
||||
// reasoning: updates at: 10 Hz, tau= 300 s settle time
|
||||
@ -303,9 +300,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
init = 0;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
|
||||
|
||||
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
@ -415,10 +412,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 * F_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
|
||||
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;
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
@ -641,7 +638,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
INSSetBaroVar(revoCalibration.baro_var);
|
||||
|
||||
// Initialize the gyro bias from the settings
|
||||
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
|
||||
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);
|
||||
|
||||
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
|
||||
@ -649,9 +646,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
|
||||
// Set initial attitude
|
||||
AttitudeActualData attitudeActual;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
|
||||
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
|
||||
@ -676,7 +673,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
INSSetMagNorth(homeLocation.Be);
|
||||
|
||||
// Initialize the gyro bias from the settings
|
||||
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
|
||||
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;
|
||||
@ -693,9 +690,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
|
||||
// Set initial attitude
|
||||
AttitudeActualData attitudeActual;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
|
||||
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
|
||||
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
|
||||
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
|
||||
@ -711,9 +708,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
|
||||
|
||||
GyrosBiasGet(&gyrosBias);
|
||||
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f,
|
||||
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f,
|
||||
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
|
||||
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};
|
||||
INSStatePrediction(gyros, &accelsData.x, dT);
|
||||
|
||||
AttitudeActualData attitude;
|
||||
@ -750,18 +747,18 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
// If the gyro bias setting was updated we should reset
|
||||
// the state estimate of the EKF
|
||||
if(gyroBiasSettingsUpdated) {
|
||||
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
|
||||
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 * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
|
||||
float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
|
||||
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
|
||||
gyros[0] += gyrosBias.x * F_PI / 180.0f;
|
||||
gyros[1] += gyrosBias.y * F_PI / 180.0f;
|
||||
gyros[2] += gyrosBias.z * F_PI / 180.0f;
|
||||
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;
|
||||
}
|
||||
|
||||
// Advance the state estimate
|
||||
@ -795,8 +792,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
|
||||
if (0) { // Old code to take horizontal velocity from GPS Position update
|
||||
sensors |= HORIZ_SENSORS;
|
||||
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
|
||||
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
|
||||
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * M_PI_F / 180.0f);
|
||||
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * M_PI_F / 180.0f);
|
||||
vel[2] = 0;
|
||||
}
|
||||
// Transform the GPS position into NED coordinates
|
||||
@ -870,9 +867,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
// 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 / F_PI;
|
||||
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
|
||||
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -295,7 +295,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
|
||||
if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
|
||||
float pitch;
|
||||
AttitudeActualPitchGet(&pitch);
|
||||
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabs(pitch))
|
||||
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch))
|
||||
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH];
|
||||
}
|
||||
break;
|
||||
@ -303,7 +303,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
|
||||
if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
|
||||
float roll;
|
||||
AttitudeActualRollGet(&roll);
|
||||
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabs(roll))
|
||||
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll))
|
||||
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL];
|
||||
}
|
||||
break;
|
||||
|
||||
@ -66,16 +66,12 @@
|
||||
#include "velocitydesired.h"
|
||||
#include "velocityactual.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#include <pios_math.h>
|
||||
#include <pios_constants.h>
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 4
|
||||
#define STACK_SIZE_BYTES 1548
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define RAD2DEG (180.0f/F_PI)
|
||||
#define DEG2RAD (F_PI/180.0f)
|
||||
#define GEE 9.81f
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static bool followerEnabled = false;
|
||||
@ -293,7 +289,7 @@ static void updatePathVelocity()
|
||||
break;
|
||||
}
|
||||
// make sure groundspeed is not zero
|
||||
if (groundspeed<1e-2) groundspeed=1e-2;
|
||||
if (groundspeed<1e-2f) groundspeed=1e-2f;
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
VelocityDesiredData velocityDesired;
|
||||
@ -313,8 +309,8 @@ static void updatePathVelocity()
|
||||
// difference between correction_direction and velocityactual >90 degrees and
|
||||
// difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything )
|
||||
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
|
||||
float angle1=RAD2DEG * ( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
|
||||
float angle2=RAD2DEG * ( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
|
||||
float angle1=RAD2DEG( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
|
||||
float angle2=RAD2DEG( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
|
||||
if (angle1<-180.0f) angle1+=360.0f;
|
||||
if (angle1>180.0f) angle1-=360.0f;
|
||||
if (angle2<-180.0f) angle2+=360.0f;
|
||||
@ -470,7 +466,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
result = 0;
|
||||
}
|
||||
|
||||
if (indicatedAirspeedActual<1e-6) {
|
||||
if (indicatedAirspeedActual<1e-6f) {
|
||||
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
|
||||
// also we cannot handle planes flying backwards, lets just wait until the nose drops
|
||||
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
|
||||
@ -583,8 +579,8 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
/**
|
||||
* Compute desired roll command
|
||||
*/
|
||||
if (groundspeedDesired> 1e-6) {
|
||||
bearingError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
|
||||
if (groundspeedDesired> 1e-6f) {
|
||||
bearingError = RAD2DEG(atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
|
||||
} else {
|
||||
// if we are not supposed to move, run in a circle
|
||||
bearingError = -90.0f;
|
||||
|
||||
@ -329,7 +329,7 @@ static uint8_t conditionTimeOut() {
|
||||
toWaypoint = waypointActive.Index;
|
||||
toStarttime = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) {
|
||||
if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) {
|
||||
// make sure we reinitialize even if the same waypoint comes twice
|
||||
toWaypoint = 0xFFFF;
|
||||
return true;
|
||||
|
||||
@ -46,20 +46,21 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include <openpilot.h>
|
||||
|
||||
#include "homelocation.h"
|
||||
#include "magnetometer.h"
|
||||
#include "magbias.h"
|
||||
#include "accels.h"
|
||||
#include "gyros.h"
|
||||
#include "gyrosbias.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "revocalibration.h"
|
||||
#include "flightstatus.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include <homelocation.h>
|
||||
#include <magnetometer.h>
|
||||
#include <magbias.h>
|
||||
#include <accels.h>
|
||||
#include <gyros.h>
|
||||
#include <gyrosbias.h>
|
||||
#include <attitudeactual.h>
|
||||
#include <attitudesettings.h>
|
||||
#include <revocalibration.h>
|
||||
#include <flightstatus.h>
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
#include <pios_math.h>
|
||||
#include <pios_board_info.h>
|
||||
|
||||
// Private constants
|
||||
@ -67,8 +68,6 @@
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
#define SENSOR_PERIOD 2
|
||||
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
|
||||
// Private types
|
||||
|
||||
|
||||
@ -302,7 +301,7 @@ static void SensorsTask(void *parameters)
|
||||
{
|
||||
struct pios_mpu6000_data mpu6000_data;
|
||||
xQueueHandle queue = PIOS_MPU6000_GetQueue();
|
||||
|
||||
|
||||
while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
|
||||
{
|
||||
gyro_accum[0] += mpu6000_data.gyro_x;
|
||||
@ -316,7 +315,7 @@ static void SensorsTask(void *parameters)
|
||||
gyro_samples ++;
|
||||
accel_samples ++;
|
||||
}
|
||||
|
||||
|
||||
if (gyro_samples == 0) {
|
||||
PIOS_MPU6000_ReadGyros(&mpu6000_data);
|
||||
error = true;
|
||||
@ -498,8 +497,8 @@ static void magOffsetEstimation(MagnetometerData *mag)
|
||||
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
|
||||
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
|
||||
|
||||
float cy = cosf(attitude.Yaw * M_PI / 180.0f);
|
||||
float sy = sinf(attitude.Yaw * M_PI / 180.0f);
|
||||
float cy = cosf(DEG2RAD(attitude.Yaw));
|
||||
float sy = sinf(DEG2RAD(attitude.Yaw));
|
||||
|
||||
xy[0] = cy * B_e[0] + sy * B_e[1];
|
||||
xy[1] = -sy * B_e[0] + cy * B_e[1];
|
||||
|
||||
@ -296,7 +296,7 @@ static void stabilizationTask(void* parameters)
|
||||
if (reinit)
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
|
||||
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
|
||||
if(fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) {
|
||||
// While getting strong commands act like rate mode
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
axis_lock_accum[i] = 0;
|
||||
@ -485,7 +485,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
|
||||
// calculation
|
||||
const float fakeDt = 0.0025;
|
||||
if(settings.GyroTau < 0.0001)
|
||||
if(settings.GyroTau < 0.0001f)
|
||||
gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
else
|
||||
gyro_alpha = expf(-fakeDt / settings.GyroTau);
|
||||
|
||||
@ -55,7 +55,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
|
||||
|
||||
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
|
||||
if (settings->VbarGyroSuppress > 0) {
|
||||
gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f);
|
||||
gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f);
|
||||
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
|
||||
}
|
||||
|
||||
|
||||
@ -460,7 +460,7 @@ static void updateStats()
|
||||
idleCounterClear = 1;
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
|
||||
float temp_voltage = 3.3 * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
|
||||
float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((float)((1 << 12) - 1));
|
||||
const float STM32_TEMP_V25 = 1.43; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
|
||||
|
||||
@ -45,46 +45,46 @@
|
||||
*/
|
||||
uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc)
|
||||
{
|
||||
if (desc)
|
||||
return PIOS_ADC_PinGet(desc->airspeedADCPin);
|
||||
return 0;
|
||||
if (desc)
|
||||
return PIOS_ADC_PinGet(desc->airspeedADCPin);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*Returns zeroPoint so that the user can inspect the calibration vs. the sensor value
|
||||
*/
|
||||
uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement){
|
||||
desc->calibrationSum += measurement;
|
||||
desc->calibrationCount++;
|
||||
desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount);
|
||||
return desc->zeroPoint;
|
||||
uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement) {
|
||||
desc->calibrationSum += measurement;
|
||||
desc->calibrationCount++;
|
||||
desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount);
|
||||
return desc->zeroPoint;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Reads the airspeed and returns CAS (calibrated airspeed) in the case of success.
|
||||
* In the case of a failed read, returns -1.
|
||||
*/
|
||||
float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement)
|
||||
{
|
||||
//Calculate dynamic pressure, as per docs
|
||||
float Qc = 3.3f/4096.0f * (float)(measurement - desc->zeroPoint);
|
||||
//Calculate dynamic pressure, as per docs
|
||||
float Qc = 3.3f/4096.0f * (float)(measurement - desc->zeroPoint);
|
||||
|
||||
//Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need
|
||||
// to saturate on the upper bound, we'll handle that later with calibratedAirspeed.
|
||||
if (Qc < 0) {
|
||||
Qc=0;
|
||||
}
|
||||
|
||||
//Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed
|
||||
float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f));
|
||||
|
||||
//Upper bound airspeed. No need to lower bound it, that comes from Qc
|
||||
if (calibratedAirspeed > desc->maxSpeed) { //in [m/s]
|
||||
calibratedAirspeed=desc->maxSpeed;
|
||||
}
|
||||
|
||||
return calibratedAirspeed;
|
||||
//Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need
|
||||
// to saturate on the upper bound, we'll handle that later with calibratedAirspeed.
|
||||
if (Qc < 0) {
|
||||
Qc=0;
|
||||
}
|
||||
|
||||
//Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed
|
||||
float calibratedAirspeed = A0 * sqrtf( 5.0f * (powf(Qc / P0 + 1.0f, POWER) - 1.0f));
|
||||
|
||||
//Upper bound airspeed. No need to lower bound it, that comes from Qc
|
||||
//in [m/s]
|
||||
if (calibratedAirspeed > desc->maxSpeed) {
|
||||
calibratedAirspeed=desc->maxSpeed;
|
||||
}
|
||||
|
||||
return calibratedAirspeed;
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_MPXV */
|
||||
|
||||
@ -1012,12 +1012,12 @@ static void PIOS_RFM22B_Task(void *parameters)
|
||||
static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
|
||||
{
|
||||
uint32_t datarate_bps = data_rate[datarate];
|
||||
rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5);
|
||||
rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
|
||||
if (rfm22_isConnected(rfm22b_dev))
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
else
|
||||
rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS;
|
||||
|
||||
134
flight/PiOS/inc/pios_constants.h
Normal file
134
flight/PiOS/inc/pios_constants.h
Normal file
@ -0,0 +1,134 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_constants.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright_F (Cf) 2013.
|
||||
* @brief Shared phisical constants
|
||||
* --
|
||||
* @see The GNU Public License_F (GPLf) 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
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
*_F (at your optionf) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_CONSTANTS_H
|
||||
#define PIOS_CONSTANTS_H
|
||||
|
||||
// Constants borrowed straight from GSL http://www.gnu.org/software/gsl/
|
||||
|
||||
#define PIOS_CONST_MKS_SPEED_OF_LIGHT_F (2.99792458e8f) /* m / s */
|
||||
#define PIOS_CONST_MKS_GRAVITATIONAL_CONSTANT_F (6.673e-11f) /* m^3 / kg s^2 */
|
||||
#define PIOS_CONST_MKS_PLANCKS_CONSTANT_H_F (6.62606896e-34f) /* kg m^2 / s */
|
||||
#define PIOS_CONST_MKS_PLANCKS_CONSTANT_HBAR_F (1.05457162825e-34f) /* kg m^2 / s */
|
||||
#define PIOS_CONST_MKS_ASTRONOMICAL_UNIT_F (1.49597870691e11f) /* m */
|
||||
#define PIOS_CONST_MKS_LIGHT_YEAR_F (9.46053620707e15f) /* m */
|
||||
#define PIOS_CONST_MKS_PARSEC_F (3.08567758135e16f) /* m */
|
||||
#define PIOS_CONST_MKS_GRAV_ACCEL_F (9.80665e0f) /* m / s^2 */
|
||||
#define PIOS_CONST_MKS_ELECTRON_VOLT_F (1.602176487e-19f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_MASS_ELECTRON_F (9.10938188e-31f) /* kg */
|
||||
#define PIOS_CONST_MKS_MASS_MUON_F (1.88353109e-28f) /* kg */
|
||||
#define PIOS_CONST_MKS_MASS_PROTON_F (1.67262158e-27f) /* kg */
|
||||
#define PIOS_CONST_MKS_MASS_NEUTRON_F (1.67492716e-27f) /* kg */
|
||||
#define PIOS_CONST_MKS_RYDBERG_F (2.17987196968e-18f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_BOLTZMANN_F (1.3806504e-23f) /* kg m^2 / K s^2 */
|
||||
#define PIOS_CONST_MKS_MOLAR_GAS_F (8.314472e0f) /* kg m^2 / K mol s^2 */
|
||||
#define PIOS_CONST_MKS_STANDARD_GAS_VOLUME_F (2.2710981e-2f) /* m^3 / mol */
|
||||
#define PIOS_CONST_MKS_MINUTE_F (6e1f) /* s */
|
||||
#define PIOS_CONST_MKS_HOUR_F (3.6e3f) /* s */
|
||||
#define PIOS_CONST_MKS_DAY_F (8.64e4f) /* s */
|
||||
#define PIOS_CONST_MKS_WEEK_F (6.048e5f) /* s */
|
||||
#define PIOS_CONST_MKS_INCH_F (2.54e-2f) /* m */
|
||||
#define PIOS_CONST_MKS_FOOT_F (3.048e-1f) /* m */
|
||||
#define PIOS_CONST_MKS_YARD_F (9.144e-1f) /* m */
|
||||
#define PIOS_CONST_MKS_MILE_F (1.609344e3f) /* m */
|
||||
#define PIOS_CONST_MKS_NAUTICAL_MILE_F (1.852e3f) /* m */
|
||||
#define PIOS_CONST_MKS_FATHOM_F (1.8288e0f) /* m */
|
||||
#define PIOS_CONST_MKS_MIL_F (2.54e-5f) /* m */
|
||||
#define PIOS_CONST_MKS_POINT_F (3.52777777778e-4f) /* m */
|
||||
#define PIOS_CONST_MKS_TEXPOINT_F (3.51459803515e-4f) /* m */
|
||||
#define PIOS_CONST_MKS_MICRON_F (1e-6f) /* m */
|
||||
#define PIOS_CONST_MKS_ANGSTROM_F (1e-10f) /* m */
|
||||
#define PIOS_CONST_MKS_HECTARE_F (1e4f) /* m^2 */
|
||||
#define PIOS_CONST_MKS_ACRE_F (4.04685642241e3f) /* m^2 */
|
||||
#define PIOS_CONST_MKS_BARN_F (1e-28f) /* m^2 */
|
||||
#define PIOS_CONST_MKS_LITER_F (1e-3f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_US_GALLON_F (3.78541178402e-3f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_QUART_F (9.46352946004e-4f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_PINT_F (4.73176473002e-4f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_CUP_F (2.36588236501e-4f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_FLUID_OUNCE_F (2.95735295626e-5f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_TABLESPOON_F (1.47867647813e-5f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_TEASPOON_F (4.92892159375e-6f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_CANADIAN_GALLON_F (4.54609e-3f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_UK_GALLON_F (4.546092e-3f) /* m^3 */
|
||||
#define PIOS_CONST_MKS_MILES_PER_HOUR_F (4.4704e-1f) /* m / s */
|
||||
#define PIOS_CONST_MKS_KILOMETERS_PER_HOUR_F (2.77777777778e-1f) /* m / s */
|
||||
#define PIOS_CONST_MKS_KNOT_F (5.14444444444e-1f) /* m / s */
|
||||
#define PIOS_CONST_MKS_POUND_MASS_F (4.5359237e-1f) /* kg */
|
||||
#define PIOS_CONST_MKS_OUNCE_MASS_F (2.8349523125e-2f) /* kg */
|
||||
#define PIOS_CONST_MKS_TON_F (9.0718474e2f) /* kg */
|
||||
#define PIOS_CONST_MKS_METRIC_TON_F (1e3f) /* kg */
|
||||
#define PIOS_CONST_MKS_UK_TON_F (1.0160469088e3f) /* kg */
|
||||
#define PIOS_CONST_MKS_TROY_OUNCE_F (3.1103475e-2f) /* kg */
|
||||
#define PIOS_CONST_MKS_CARAT_F (2e-4f) /* kg */
|
||||
#define PIOS_CONST_MKS_UNIFIED_ATOMIC_MASS_F (1.660538782e-27f) /* kg */
|
||||
#define PIOS_CONST_MKS_GRAM_FORCE_F (9.80665e-3f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_POUND_FORCE_F (4.44822161526e0f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_KILOPOUND_FORCE_F (4.44822161526e3f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_POUNDAL_F (1.38255e-1f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_CALORIE_F (4.1868e0f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_BTU_F (1.05505585262e3f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_THERM_F (1.05506e8f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_HORSEPOWER_F (7.457e2f) /* kg m^2 / s^3 */
|
||||
#define PIOS_CONST_MKS_BAR_F (1e5f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_STD_ATMOSPHERE_F (1.01325e5f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_TORR_F (1.33322368421e2f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_METER_OF_MERCURY_F (1.33322368421e5f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_INCH_OF_MERCURY_F (3.38638815789e3f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_INCH_OF_WATER_F (2.490889e2f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_PSI_F (6.89475729317e3f) /* kg / m s^2 */
|
||||
#define PIOS_CONST_MKS_POISE_F (1e-1f) /* kg m^-1 s^-1 */
|
||||
#define PIOS_CONST_MKS_STOKES_F (1e-4f) /* m^2 / s */
|
||||
#define PIOS_CONST_MKS_STILB_F (1e4f) /* cd / m^2 */
|
||||
#define PIOS_CONST_MKS_LUMEN_F (1e0f) /* cd sr */
|
||||
#define PIOS_CONST_MKS_LUX_F (1e0f) /* cd sr / m^2 */
|
||||
#define PIOS_CONST_MKS_PHOT_F (1e4f) /* cd sr / m^2 */
|
||||
#define PIOS_CONST_MKS_FOOTCANDLE_F (1.076e1f) /* cd sr / m^2 */
|
||||
#define PIOS_CONST_MKS_LAMBERT_F (1e4f) /* cd sr / m^2 */
|
||||
#define PIOS_CONST_MKS_FOOTLAMBERT_F (1.07639104e1f) /* cd sr / m^2 */
|
||||
#define PIOS_CONST_MKS_CURIE_F (3.7e10f) /* 1 / s */
|
||||
#define PIOS_CONST_MKS_ROENTGEN_F (2.58e-4f) /* A s / kg */
|
||||
#define PIOS_CONST_MKS_RAD_F (1e-2f) /* m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_SOLAR_MASS_F (1.98892e30f) /* kg */
|
||||
#define PIOS_CONST_MKS_BOHR_RADIUS_F (5.291772083e-11f) /* m */
|
||||
#define PIOS_CONST_MKS_NEWTON_F (1e0f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_DYNE_F (1e-5f) /* kg m / s^2 */
|
||||
#define PIOS_CONST_MKS_JOULE_F (1e0f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_ERG_F (1e-7f) /* kg m^2 / s^2 */
|
||||
#define PIOS_CONST_MKS_STEFAN_BOLTZMANN_CONSTANT_F (5.67040047374e-8f) /* kg / K^4 s^3 */
|
||||
#define PIOS_CONST_MKS_THOMSON_CROSS_SECTION_F (6.65245893699e-29f) /* m^2 */
|
||||
#define PIOS_CONST_MKS_BOHR_MAGNETON_F (9.27400899e-24f) /* A m^2 */
|
||||
#define PIOS_CONST_MKS_NUCLEAR_MAGNETON_F (5.05078317e-27f) /* A m^2 */
|
||||
#define PIOS_CONST_MKS_ELECTRON_MAGNETIC_MOMENT_F (9.28476362e-24f) /* A m^2 */
|
||||
#define PIOS_CONST_MKS_PROTON_MAGNETIC_MOMENT_F (1.410606633e-26f) /* A m^2 */
|
||||
#define PIOS_CONST_MKS_FARADAY_F (9.64853429775e4f) /* A s / mol */
|
||||
#define PIOS_CONST_MKS_ELECTRON_CHARGE_F (1.602176487e-19f) /* A s */
|
||||
#define PIOS_CONST_MKS_VACUUM_PERMITTIVITY_F (8.854187817e-12f) /* A^2 s^4 / kg m^3 */
|
||||
#define PIOS_CONST_MKS_VACUUM_PERMEABILITY_F (1.25663706144e-6f) /* kg m / A^2 s^2 */
|
||||
#define PIOS_CONST_MKS_DEBYE_F (3.33564095198e-30f) /* A s^2 / m^2 */
|
||||
#define PIOS_CONST_MKS_GAUSS_F (1e-4f) /* kg / A s^2 */
|
||||
|
||||
#endif /* PIOS_CONSTANTS_H */
|
||||
40
flight/PiOS/inc/pios_helpers.h
Normal file
40
flight/PiOS/inc/pios_helpers.h
Normal file
@ -0,0 +1,40 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_helpers.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Header for helper functions/macro definitions
|
||||
*
|
||||
* @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
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_HELPERS_H
|
||||
#define PIOS_HELPERS_H
|
||||
|
||||
|
||||
/**
|
||||
* @brief return the number of elements contained in the array x.
|
||||
* @param[in] x the array
|
||||
* @return number of elements in x.
|
||||
*
|
||||
*/
|
||||
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
|
||||
|
||||
|
||||
#endif // PIOS_HELPERS_H
|
||||
56
flight/PiOS/inc/pios_math.h
Normal file
56
flight/PiOS/inc/pios_math.h
Normal file
@ -0,0 +1,56 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_math.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Header for math functions/macro definitions
|
||||
*
|
||||
* @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
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef PIOS_MATH_H
|
||||
#define PIOS_MATH_H
|
||||
// Generic float math constants
|
||||
#define M_E_F 2.71828182845904523536028747135f /* e */
|
||||
#define M_LOG2E_F 1.44269504088896340735992468100f /* log_2 (e) */
|
||||
#define M_LOG10E_F 0.43429448190325182765112891892f /* log_10 (e) */
|
||||
#define M_SQRT2_F 1.41421356237309504880168872421f /* sqrt(2) */
|
||||
#define M_SQRT1_2_F 0.70710678118654752440084436210f /* sqrt(1/2) */
|
||||
#define M_SQRT3_F 1.73205080756887729352744634151f /* sqrt(3) */
|
||||
#define M_PI_F 3.14159265358979323846264338328f /* pi */
|
||||
#define M_PI_2_F 1.57079632679489661923132169164f /* pi/2 */
|
||||
#define M_PI_4_F 0.78539816339744830961566084582f /* pi/4 */
|
||||
#define M_SQRTPI_F 1.77245385090551602729816748334f /* sqrt(pi) */
|
||||
#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */
|
||||
#define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */
|
||||
#define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */
|
||||
#define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */
|
||||
#define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */
|
||||
#define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */
|
||||
#define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */
|
||||
|
||||
// Conversion macro
|
||||
#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F))
|
||||
#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f))
|
||||
|
||||
// Useful math macros
|
||||
#define MAX(a,b) ((a) > (b) ? (a) : (b))
|
||||
#define MIN(a,b) ((a) < (b) ? (a) : (b))
|
||||
|
||||
#endif // PIOS_MATH_H
|
||||
@ -34,8 +34,7 @@
|
||||
#ifndef PIOS_H
|
||||
#define PIOS_H
|
||||
|
||||
/* Used in pios_*.* files macro */
|
||||
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
|
||||
#include <pios_helpers.h>
|
||||
|
||||
#ifdef USE_SIM_POSIX
|
||||
/* SimPosix version of this file. This will probably be removed later */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user