Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

helper_functions: generalize unwrapping function

Browse Source
This commit is contained in:
Matthias Grob
2022-02-25 17:22:31 +01:00
parent c86c2db07f
commit c4bc062714
2 changed files with 23 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/helper_functions.hpp
+19 -3
View File
@@ -124,16 +124,32 @@ Type wrap_2pi(Type x)
}
/**
* Unwrap angles
* Unwrap value that was wrapped with range [low, high)
*
* @param[in] last_x Last unwrapped value
* @param[in] new_x New value in range
* @param low lower limit of the wrapping range
* @param high upper limit of the wrapping range
* @return New unwrapped value
*/
template<typename Type>
Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high)
{
return last_x + wrap(new_x - last_x, low, high);
}
/**
* Unwrap value with range [-π, π)
*
* @param[in] last_angle Last unwrapped angle [rad]
* @param[in] new_angle New angle in [-pi, pi] [rad]
* @param
* @return New unwrapped angle [rad]
*/
template<typename Type>
Type unwrap(const Type last_angle, const Type new_angle)
Type unwrap_pi(const Type last_angle, const Type new_angle)
{
return last_angle + wrap_pi(new_angle - last_angle);
return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
}
template<typename T>
src/lib/matrix/test/unwrap.cpp
+4 -4
View File
@@ -19,7 +19,7 @@ TEST(Unwrap, UnwrapFloats)
float last_angle = wrapped_angles[0];
for (int i = 1; i < 6; i++) {
last_angle = unwrap(last_angle, wrapped_angles[i]);
last_angle = unwrap_pi(last_angle, wrapped_angles[i]);
EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]);
}
@@ -27,7 +27,7 @@ TEST(Unwrap, UnwrapFloats)
last_angle = -wrapped_angles[0];
for (int i = 1; i < 6; i++) {
last_angle = unwrap(last_angle, -wrapped_angles[i]);
last_angle = unwrap_pi(last_angle, -wrapped_angles[i]);
EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]);
}
}
@@ -48,7 +48,7 @@ TEST(Unwrap, UnwrapDoubles)
double last_angle = wrapped_angles[0];
for (int i = 1; i < 6; i++) {
last_angle = unwrap(last_angle, wrapped_angles[i]);
last_angle = unwrap_pi(last_angle, wrapped_angles[i]);
EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]);
}
@@ -56,7 +56,7 @@ TEST(Unwrap, UnwrapDoubles)
last_angle = -wrapped_angles[0];
for (int i = 1; i < 6; i++) {
last_angle = unwrap(last_angle, -wrapped_angles[i]);
last_angle = unwrap_pi(last_angle, -wrapped_angles[i]);
EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]);
}
}