fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 06:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

helper_functions: simplify unwrap function

Browse Source
This commit is contained in:
Matthias Grob
2022-03-08 10:56:32 +01:00
parent 5a3aba9c21
commit c86c2db07f
1 changed files with 1 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/helper_functions.hpp
+1 -9
View File
@@ -133,15 +133,7 @@ Type wrap_2pi(Type x)
template<typename Type> template<typename Type>
Type unwrap(const Type last_angle, const Type new_angle) Type unwrap(const Type last_angle, const Type new_angle)
{ {
// wrap the last angle in [-pi,pi] return last_angle + wrap_pi(new_angle - last_angle);
const Type last_angle_wrapped = matrix::detail::wrap_floating(last_angle, -Type(M_PI), Type(M_PI));
// use the shortest distance
Type delta = new_angle - last_angle_wrapped;
delta += ((delta < -Type(M_PI)) - (delta > Type(M_PI))) * Type(2 *
M_PI); // adds or subtracts 2*pi if delta out of range
return delta + last_angle;
} }
template<typename T> template<typename T>