Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added missing 90 degree rotations 

this ensures we can handle all 90 degree rotations of the compass and
main board. A test in examples/rotations shows that we have them all.
This commit is contained in:
Andrew Tridgell 2013-08-26 23:02:48 +10:00
parent c6c59174b3
commit a295a01bbc
3 changed files with 119 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Math/examples/rotations/rotations.pde
View File

@ -295,6 +295,50 @@ static void test_eulers(void)
test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135);
test_euler(ROTATION_PITCH_90, 0, 90, 0);
test_euler(ROTATION_PITCH_270, 0, 270, 0);
test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90);
test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270);
test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0);
test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0);
test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0);
test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0);
test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0);
test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0);
test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0);
test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0);
test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90);
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270);
}
static bool have_rotation(const Matrix3f &m)
{
Matrix3f mt = m.transposed();
for (enum Rotation r=ROTATION_NONE;
r<ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) {
Vector3f v(1,2,3);
Vector3f v2 = v;
v2.rotate(r);
v2 = mt * v2;
if ((v2 - v).length() < 0.01f) {
return true;
}
}
return false;
}
static void missing_rotations(void)
{
hal.console->println("testing for missing rotations");
uint16_t roll, pitch, yaw;
for (yaw=0; yaw<360; yaw += 90)
for (pitch=0; pitch<360; pitch += 90)
for (roll=0; roll<360; roll += 90) {
Matrix3f m;
m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw));
if (!have_rotation(m)) {
hal.console->printf("Missing rotation (%u, %u, %u)\n", roll, pitch, yaw);
}
}
}
/*
@ -310,6 +354,7 @@ void setup(void)
#endif
test_rotation_accuracy();
test_eulers();
missing_rotations();
hal.console->println("rotation unit tests done\n");
}

14
libraries/AP_Math/rotations.h
View File

@ -52,11 +52,23 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_PITCH_180_YAW_90 = 26,
ROTATION_PITCH_180_YAW_270 = 27,
ROTATION_ROLL_90_PITCH_90 = 28,
ROTATION_ROLL_180_PITCH_90 = 29,
ROTATION_ROLL_270_PITCH_90 = 30,
ROTATION_ROLL_90_PITCH_180 = 31,
ROTATION_ROLL_270_PITCH_180 = 32,
ROTATION_ROLL_90_PITCH_270 = 33,
ROTATION_ROLL_180_PITCH_270 = 34,
ROTATION_ROLL_270_PITCH_270 = 35,
ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
ROTATION_ROLL_90_YAW_270 = 37,
ROTATION_MAX
};
/*
Here are the same values in a form sutable for a @Values attribute in
auto documentation:
@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270
@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270
*/

61
libraries/AP_Math/vector3.cpp
View File

@ -160,6 +160,67 @@ void Vector3<T>::rotate(enum Rotation rotation)
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_PITCH_180_YAW_90: {
z = -z;
tmp = -x; x = -y; y = tmp;
return;
}
case ROTATION_PITCH_180_YAW_270: {
x = -x; z = -z;
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_ROLL_90_PITCH_90: {
tmp = z; z = y; y = -tmp;
tmp = z; z = -x; x = tmp;
return;
}
case ROTATION_ROLL_180_PITCH_90: {
y = -y; z = -z;
tmp = z; z = -x; x = tmp;
return;
}
case ROTATION_ROLL_270_PITCH_90: {
tmp = z; z = -y; y = tmp;
tmp = z; z = -x; x = tmp;
return;
}
case ROTATION_ROLL_90_PITCH_180: {
tmp = z; z = y; y = -tmp;
x = -x; z = -z;
return;
}
case ROTATION_ROLL_270_PITCH_180: {
tmp = z; z = -y; y = tmp;
x = -x; z = -z;
return;
}
case ROTATION_ROLL_90_PITCH_270: {
tmp = z; z = y; y = -tmp;
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_ROLL_180_PITCH_270: {
y = -y; z = -z;
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_ROLL_270_PITCH_270: {
tmp = z; z = -y; y = tmp;
tmp = z; z = x; x = -tmp;
return;
}
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
tmp = z; z = y; y = -tmp;
x = -x; z = -z;
tmp = x; x = -y; y = tmp;
return;
}
case ROTATION_ROLL_90_YAW_270: {
tmp = z; z = y; y = -tmp;
tmp = x; x = y; y = -tmp;
return;
}
}
}