aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: examples: test custom rotations

Browse Source
This commit is contained in:
Iampete1 2021-11-20 23:55:37 +00:00 committed by Andrew Tridgell
parent 15326a8d90
commit 97602aadf5
1 changed files with 36 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Math/examples/rotations/rotations.cpp
View File

@ -4,6 +4,9 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_CustomRotations/AP_CustomRotations.h>
AP_CustomRotations cust_rot;
void setup(); void setup();
void loop(); void loop();
@ -165,9 +168,9 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya
float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw; float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw;
q.to_euler(q_roll, q_pitch, q_yaw); q.to_euler(q_roll, q_pitch, q_yaw);
qe.to_euler(qe_roll, qe_pitch, qe_yaw); qe.to_euler(qe_roll, qe_pitch, qe_yaw);
const float roll_diff = fabsf(wrap_PI(q_roll - qe_roll)); float roll_diff = fabsf(wrap_PI(q_roll - qe_roll));
const float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch)); float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch));
const float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw)); float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw));
if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) { if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) {
hal.console->printf("quaternion test %u failed : yaw:%f/%f roll:%f/%f pitch:%f/%f\n", hal.console->printf("quaternion test %u failed : yaw:%f/%f roll:%f/%f pitch:%f/%f\n",
(unsigned)rotation, (unsigned)rotation,
@ -175,6 +178,36 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya
(double)q_roll,(double)qe_roll, (double)q_roll,(double)qe_roll,
(double)q_pitch,(double)qe_pitch); (double)q_pitch,(double)qe_pitch);
} }
// test custom rotations
AP::custom_rotations().set(ROTATION_CUSTOM_1, roll, pitch, yaw);
v1 = v;
v1.rotate(ROTATION_CUSTOM_1);
diff = (v2 - v1);
if (diff.length() > accuracy) {
hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n",
(unsigned)rotation,
(int)yaw,
(int)roll,
(int)pitch);
hal.console->printf("custom rotated: ");
print_vector(v1);
hal.console->printf("correct rotated: ");
print_vector(v2);
hal.console->printf("\n");
}
Quaternion qc;
qc.from_rotation(ROTATION_CUSTOM_1);
float qc_roll, qc_pitch, qc_yaw;
qc.to_euler(qc_roll, qc_pitch, qc_yaw);
roll_diff = fabsf(wrap_PI(qc_roll - qe_roll));
pitch_diff = fabsf(wrap_PI(qc_pitch - qe_pitch));
yaw_diff = fabsf(wrap_PI(qc_yaw - qe_yaw));
if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) {
hal.console->printf("custom quaternion test %u failed\n", (unsigned)rotation);
}
} }
static void test_rotate_inverse(void) static void test_rotate_inverse(void)