Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: use ftype for a few internal trig fns

Browse Source
This commit is contained in:
Andrew Tridgell 2021-07-09 12:20:50 +10:00
parent a6a01cf432
commit afb928081a
4 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/AP_Math.h
View File

@ -143,13 +143,13 @@ double wrap_360_cd(const double angle);
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
template <typename T>
float wrap_PI(const T radian);
ftype wrap_PI(const T radian);
/*
* wrap an angle in radians to 0..2PI
*/
template <typename T>
float wrap_2PI(const T radian);
ftype wrap_2PI(const T radian);
/*
* Constrain a value to be within the range: low and high
@ -179,7 +179,7 @@ inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64
}
// degrees -> radians
static inline constexpr float radians(float deg)
static inline constexpr ftype radians(ftype deg)
{
return deg * DEG_TO_RAD;
}

2
libraries/AP_Math/definitions.h
View File

@ -7,7 +7,7 @@
#ifdef M_PI
# undef M_PI
#endif
#define M_PI (3.141592653589793f)
#define M_PI (3.141592653589793)
#ifdef M_PI_2
# undef M_PI_2

8
libraries/AP_Math/matrix3.cpp
View File

@ -52,10 +52,10 @@ void Matrix3<T>::to_euler(T *roll, T *pitch, T *yaw) const
*pitch = -safe_asin(c.x);
}
if (roll != nullptr) {
*roll = atan2f(c.y, c.z);
*roll = atan2F(c.y, c.z);
}
if (yaw != nullptr) {
*yaw = atan2f(b.x, a.x);
*yaw = atan2F(b.x, a.x);
}
}
@ -81,8 +81,8 @@ template <typename T>
Vector3<T> Matrix3<T>::to_euler312() const
{
return Vector3<T>(asinF(c.y),
atan2f(-c.x, c.z),
atan2f(-a.y, b.y));
atan2F(-c.x, c.z),
atan2F(-a.y, b.y));
}
/*

20
libraries/AP_Math/quaternion.cpp
View File

@ -481,9 +481,9 @@ void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
q2=q3=q4=0.0f;
return;
}
const T st2 = sinF(theta/2.0f);
const T st2 = sinF(0.5*theta);
q1 = cosF(theta/2.0f);
q1 = cosF(0.5*theta);
q2 = axis.x * st2;
q3 = axis.y * st2;
q4 = axis.z * st2;
@ -507,7 +507,7 @@ void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
v = Vector3<T>(q2,q3,q4);
if (!is_zero(l)) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
v *= wrap_PI(2.0f * atan2F(l,q1));
}
}
@ -531,11 +531,11 @@ void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
template <typename T>
void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
{
const T t2 = theta/2.0f;
const T t2 = 0.5*theta;
const T sqt2 = sq(t2);
const T st2 = t2-sqt2*t2/6.0f;
q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
q1 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
q2 = axis.x * st2;
q3 = axis.y * st2;
q4 = axis.z * st2;
@ -550,13 +550,13 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
if (is_zero(theta)) {
return;
}
const T t2 = theta/2.0f;
const T t2 = 0.5*theta;
const T sqt2 = sq(t2);
T st2 = t2-sqt2*t2/6.0f;
st2 /= theta;
//"rotation quaternion"
const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
const T x2 = v.x * st2;
const T y2 = v.y * st2;
const T z2 = v.z * st2;
@ -578,7 +578,7 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
template <typename T>
T QuaternionT<T>::get_euler_roll() const
{
return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
return (atan2F(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
}
// get euler pitch angle
@ -592,7 +592,7 @@ T QuaternionT<T>::get_euler_pitch() const
template <typename T>
T QuaternionT<T>::get_euler_yaw() const
{
return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
return atan2F(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
}
// create eulers from a quaternion
@ -772,7 +772,7 @@ T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
// calculate and return angular difference
return (2.0 * asinf(vec_len_div2));
return (2.0 * asinF(vec_len_div2));
}
// define for float and double