Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add separate double and float implementations for wrap_PI and wrap_2PI

Browse Source 
avoids double promotion when the argument is not double
This commit is contained in:
Peter Barker
2026-01-28 19:17:49 +11:00
committed by Andrew Tridgell
parent aeacb3b6fd
commit 0c26780cce
3 changed files with 33 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Math/AP_Math.cpp
+23 -6
View File
@@ -248,23 +248,40 @@ long wrap_360_cd(const long angle)
return res;
}
ftype wrap_PI(const ftype radian)
float wrap_2PI(const float radian)
{
ftype res = wrap_2PI(radian);
if (res > M_PI) {
res -= M_2PI;
float res = fmodf(radian, float(M_2PI));
if (res < 0) {
res += float(M_2PI);
}
return res;
}
ftype wrap_2PI(const ftype radian)
#if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
double wrap_2PI(const double radian)
{
ftype res = fmodF(radian, M_2PI);
double res = fmod(radian, M_2PI);
if (res < 0) {
res += M_2PI;
}
return res;
}
#endif
template <typename T>
T wrap_PI(const T radian)
{
T res = wrap_2PI(radian);
if (res > T(M_PI)) {
res -= T(M_2PI);
}
return res;
}
template float wrap_PI<float>(const float radian);
#if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
template double wrap_PI<double>(const double radian);
#endif
template <typename T>
T constrain_value_line(const T amt, const T low, const T high, uint32_t line)