Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added rotation_equal()

Browse Source
This commit is contained in:
Andrew Tridgell 2018-10-01 13:04:11 +10:00 committed by Randy Mackay
parent 2ad2bb43f8
commit e90e89b687
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Math/AP_Math.cpp
View File

@ -255,3 +255,21 @@ bool is_valid_octal(uint16_t octal)
} }
return true; return true;
} }
/*
return true if two rotations are equivalent
This copes with the fact that we have some duplicates, like ROLL_180_YAW_90 and PITCH_180_YAW_270
*/
bool rotation_equal(enum Rotation r1, enum Rotation r2)
{
if (r1 == r2) {
return true;
}
Vector3f v(1,2,3);
Vector3f v1 = v;
Vector3f v2 = v;
v1.rotate(r1);
v2.rotate(r2);
return (v1 - v2).length() < 0.001;
}

2
libraries/AP_Math/AP_Math.h
View File

@ -258,3 +258,5 @@ Vector3f rand_vec3f(void);
// confirm a value is a valid octal value // confirm a value is a valid octal value
bool is_valid_octal(uint16_t octal); bool is_valid_octal(uint16_t octal);
// return true if two rotations are equal
bool rotation_equal(enum Rotation r1, enum Rotation r2);