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

A smaller codesize wrap, since it gets inlined in many places

Browse Source
This commit is contained in:
Julian Kent 2019-09-05 17:34:29 +02:00 committed by Matthias Grob
parent 74ace7d1d5
commit 22bf63cb71
2 changed files with 5 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
matrix/helper_functions.hpp
View File

@ -26,7 +26,7 @@ bool is_finite(Type x) {
* @param x input possibly outside of the range
* @param low lower limit of the allowed range
* @param high upper limit of the allowed range
* @return wrapped value inside the range, or NAN if value is too far away from range.
* @return wrapped value inside the range
*/
template<typename Type>
Type wrap(Type x, Type low, Type high) {
@ -35,24 +35,10 @@ Type wrap(Type x, Type low, Type high) {
return x;
}
// close to range
Type range = high - low;
if ((high <= x) && (x < high + (range*100))) {
while (high <= x) {
x -= range;
}
return x;
}
if ((low - (range*100) <= x) && (x < low)) {
while (x < low) {
x += range;
}
return x;
}
// very far from the range -> something went terribly wrong
return NAN;
const Type range = high - low;
const Type inv_range = Type(1) / range; // should evaluate at compile time, multiplies below at runtime
const Type num_wraps = floor((x - low) * inv_range);
return x - range * num_wraps;
}
/**

1
test/helper.cpp
View File

@ -20,7 +20,6 @@ int main()
TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
TEST(!is_finite(wrap(1000., 0., .01)));
// wrap pi
TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);