mirror of https://github.com/ArduPilot/ardupilot
AP_Math: remove check for AVR CPUs
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be removed on separate commits.
This commit is contained in:
parent
255e04a841
commit
fb28f426da
|
@ -37,11 +37,9 @@
|
|||
|
||||
//GPS Specific double precision conversions
|
||||
//The precision here does matter when using the wsg* functions for converting
|
||||
//between LLH and ECEF coordinates. Test code in examlpes/location/location.pde
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
#define DEG_TO_RAD_DOUBLE 0.0174532925199432954743716805978692718781530857086181640625 // equals to (M_PI / 180.0)
|
||||
#define RAD_TO_DEG_DOUBLE 57.29577951308232286464772187173366546630859375 // equals to (180.0 / M_PI)
|
||||
#endif
|
||||
//between LLH and ECEF coordinates. Test code in examlpes/location/location.cpp
|
||||
#define DEG_TO_RAD_DOUBLE 0.0174532925199432954743716805978692718781530857086181640625 // equals to (M_PI / 180.0)
|
||||
#define RAD_TO_DEG_DOUBLE 57.29577951308232286464772187173366546630859375 // equals to (180.0 / M_PI)
|
||||
|
||||
#define RadiansToCentiDegrees(x) ((x) * 5729.5779513082320876798154814105f)
|
||||
|
||||
|
@ -157,17 +155,15 @@ bool locations_are_same(const struct Location &loc1, const struct Location &loc2
|
|||
*/
|
||||
void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon);
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
// Converts from WGS84 geodetic coordinates (lat, lon, height)
|
||||
// into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates
|
||||
// (X, Y, Z)
|
||||
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef);
|
||||
|
||||
// Converts from WGS84 Earth Centered, Earth Fixed (ECEF)
|
||||
// coordinates (X, Y, Z), into WHS84 geodetic
|
||||
// Converts from WGS84 Earth Centered, Earth Fixed (ECEF)
|
||||
// coordinates (X, Y, Z), into WHS84 geodetic
|
||||
// coordinates (lat, lon, height)
|
||||
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh);
|
||||
#endif
|
||||
|
||||
// constrain a value
|
||||
// constrain a value
|
||||
|
|
|
@ -234,7 +234,6 @@ static void test_wrap_cd(void)
|
|||
hal.console->printf("wrap_cd tests done\n");
|
||||
}
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
static void test_wgs_conversion_functions(void)
|
||||
{
|
||||
|
||||
|
@ -319,7 +318,6 @@ static void test_wgs_conversion_functions(void)
|
|||
|
||||
}
|
||||
}
|
||||
#endif //HAL_CPU_CLASS
|
||||
|
||||
/*
|
||||
* polygon tests
|
||||
|
@ -330,9 +328,7 @@ void setup(void)
|
|||
test_offset();
|
||||
test_accuracy();
|
||||
test_wrap_cd();
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
test_wgs_conversion_functions();
|
||||
#endif
|
||||
hal.console->printf("ALL TESTS DONE\n");
|
||||
}
|
||||
|
||||
|
|
|
@ -246,8 +246,6 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
|
|||
s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion);
|
||||
}
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
|
||||
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) {
|
||||
double d = WGS84_E * sin(llh[0]);
|
||||
double N = WGS84_A / sqrt(1. - d*d);
|
||||
|
@ -349,6 +347,3 @@ 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
|
||||
|
||||
|
|
|
@ -244,7 +244,6 @@ template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) cons
|
|||
template Matrix3<float> Matrix3<float>::transposed(void) const;
|
||||
template Vector2<float> Matrix3<float>::mulXY(const Vector3<float> &v) const;
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
template void Matrix3<double>::zero(void);
|
||||
template void Matrix3<double>::rotate(const Vector3<double> &g);
|
||||
template void Matrix3<double>::rotateXY(const Vector3<double> &g);
|
||||
|
@ -256,4 +255,3 @@ template Vector3<double> Matrix3<double>::mul_transpose(const Vector3<double> &v
|
|||
template Matrix3<double> Matrix3<double>::operator *(const Matrix3<double> &m) const;
|
||||
template Matrix3<double> Matrix3<double>::transposed(void) const;
|
||||
template Vector2<double> Matrix3<double>::mulXY(const Vector3<double> &v) const;
|
||||
#endif
|
||||
|
|
|
@ -241,8 +241,6 @@ typedef Matrix3<uint16_t> Matrix3ui;
|
|||
typedef Matrix3<int32_t> Matrix3l;
|
||||
typedef Matrix3<uint32_t> Matrix3ul;
|
||||
typedef Matrix3<float> Matrix3f;
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
typedef Matrix3<double> Matrix3d;
|
||||
#endif
|
||||
typedef Matrix3<double> Matrix3d;
|
||||
|
||||
#endif // MATRIX3_H
|
||||
|
|
|
@ -385,7 +385,6 @@ template bool Vector3<float>::is_nan(void) const;
|
|||
template bool Vector3<float>::is_inf(void) const;
|
||||
template float Vector3<float>::angle(const Vector3<float> &v) const;
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
template void Vector3<double>::rotate(enum Rotation);
|
||||
template float Vector3<double>::length(void) const;
|
||||
template Vector3<double> Vector3<double>::operator %(const Vector3<double> &v) const;
|
||||
|
@ -406,4 +405,3 @@ template bool Vector3<double>::operator !=(const Vector3<double> &v) const;
|
|||
template bool Vector3<double>::is_nan(void) const;
|
||||
template bool Vector3<double>::is_inf(void) const;
|
||||
template float Vector3<double>::angle(const Vector3<double> &v) const;
|
||||
#endif
|
||||
|
|
|
@ -217,8 +217,6 @@ typedef Vector3<uint16_t> Vector3ui;
|
|||
typedef Vector3<int32_t> Vector3l;
|
||||
typedef Vector3<uint32_t> Vector3ul;
|
||||
typedef Vector3<float> Vector3f;
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
typedef Vector3<double> Vector3d;
|
||||
#endif
|
||||
typedef Vector3<double> Vector3d;
|
||||
|
||||
#endif // VECTOR3_H
|
||||
|
|
Loading…
Reference in New Issue