aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: allow double precision maths where needed

Browse Source
This commit is contained in:
Andrew Tridgell 2018-05-04 11:19:31 +10:00
parent 53d36c02ae
commit 3b8ec3a2a0
3 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Math/definitions.h
View File

@ -36,8 +36,10 @@
// GPS Specific double precision conversions
// The precision here does matter when using the wsg* functions for converting
// between LLH and ECEF coordinates.
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
static const double DEG_TO_RAD_DOUBLE = asin(1) / 90;
static const double RAD_TO_DEG_DOUBLE = 1 / DEG_TO_RAD_DOUBLE;
#endif
#define RadiansToCentiDegrees(x) (static_cast<float>(x) * RAD_TO_DEG * static_cast<float>(100))
@ -65,7 +67,9 @@ static const double WGS84_F = ((double)1.0 / WGS84_IF);
static const double WGS84_B = (WGS84_A * (1 - WGS84_F));
// Eccentricity of the Earth
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F));
#endif
// air density at 15C at sea level in kg/m^3
#define AIR_DENSITY_SEA_LEVEL 1.225f

5
libraries/AP_Math/location.cpp
View File

@ -211,6 +211,10 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion);
}
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
/*
these are not currently used. They should be moved to location_double.cpp if we do enable them in the future
*/
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) {
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1 - d*d);
@ -312,6 +316,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
}
#endif // ALLOW_DOUBLE_MATH_FUNCTIONS
// return true when lat and lng are within range
bool check_lat(float lat)

2
libraries/AP_Math/quaternion.cpp
View File

@ -188,7 +188,7 @@ void Quaternion::rotate(const Vector3f &v)
void Quaternion::to_axis_angle(Vector3f &v)
{
float l = sqrt(sq(q2)+sq(q3)+sq(q4));
float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4);
if (!is_zero(l)) {
v /= l;