fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

helper_functions: simplify unwrap function

Browse Source
This commit is contained in:
Matthias Grob 2022-02-25 16:41:07 +01:00
parent 5a3aba9c21
commit c86c2db07f
1 changed files with 1 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/matrix/matrix/helper_functions.hpp
View File

@ -133,15 +133,7 @@ Type wrap_2pi(Type x)
template<typename Type>
Type unwrap(const Type last_angle, const Type new_angle)
{
// wrap the last angle in [-pi,pi]
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;
return last_angle + wrap_pi(new_angle - last_angle);
}
template<typename T>