Skip to content

Commit fe842d9

Browse files
committed
AP_Math: test trim composition matches the rotation-matrix form
AP_AHRS now applies the board-mounting trim to a backend's attitude by composing its quaternion with from_euler(trim).inverse() and deriving the rotation matrix from the result, rather than keeping a separately-trimmed matrix. Assert that this yields the same rotation matrix as the previous form, R_backend * rotation_vehicle_body_to_autopilot_body, across a range of attitudes and a trim that is non-zero on all three axes at once.
1 parent 45c942d commit fe842d9

1 file changed

Lines changed: 55 additions & 0 deletions

File tree

‎libraries/AP_Math/tests/test_quaternion.cpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -251,4 +251,59 @@ TEST(QuaternionTest, Quaternion_length_squared)
251251
EXPECT_FLOAT_EQ(q.length_squared(), 1.44);
252252
}
253253

254+
// AP_AHRS applies the board-mounting trim to each backend's attitude by
255+
// composing the NED-from-autopilot-body quaternion with
256+
// get_quat_vehicle_body_to_autopilot_body(), which is
257+
// from_euler(trim).inverse(), and deriving the rotation matrix from the
258+
// result. That must be identical to the historical matrix form
259+
// R_backend * rotation_vehicle_body_to_autopilot_body (where that matrix
260+
// is from_euler(trim).transposed()), including for a trim that is non-zero
261+
// on all three axes at once.
262+
TEST(QuaternionTest, TrimCompositionMatchesRotationMatrix)
263+
{
264+
const Vector3f attitudes[] {
265+
{0.0f, 0.0f, 0.0f},
266+
{radians(10), radians(-20), radians(30)},
267+
{radians(-45), radians(60), radians(170)},
268+
{radians(80), radians(-85), radians(-120)},
269+
};
270+
const Vector3f trims[] {
271+
{0.0f, 0.0f, 0.0f},
272+
{radians(3), radians(-2), 0.0f},
273+
{radians(5), radians(4), radians(6)}, // all three axes at once
274+
};
275+
for (const auto &att : attitudes) {
276+
Quaternion q_backend;
277+
q_backend.from_euler(att);
278+
Matrix3f R_backend;
279+
q_backend.rotation_matrix(R_backend);
280+
for (const auto &trim : trims) {
281+
// the trim quaternion and matrix as AP_AHRS builds them:
282+
Quaternion q_trim;
283+
q_trim.from_euler(trim);
284+
q_trim = q_trim.inverse();
285+
Matrix3f rot_autopilot_to_vehicle;
286+
rot_autopilot_to_vehicle.from_euler(trim.x, trim.y, trim.z);
287+
const Matrix3f rot_vehicle_to_autopilot = rot_autopilot_to_vehicle.transposed();
288+
289+
Quaternion q_vehicle = q_backend;
290+
q_vehicle *= q_trim;
291+
Matrix3f R_from_quat;
292+
q_vehicle.rotation_matrix(R_from_quat);
293+
294+
const Matrix3f R_expected = R_backend * rot_vehicle_to_autopilot;
295+
296+
EXPECT_NEAR(R_from_quat.a.x, R_expected.a.x, 1e-4f);
297+
EXPECT_NEAR(R_from_quat.a.y, R_expected.a.y, 1e-4f);
298+
EXPECT_NEAR(R_from_quat.a.z, R_expected.a.z, 1e-4f);
299+
EXPECT_NEAR(R_from_quat.b.x, R_expected.b.x, 1e-4f);
300+
EXPECT_NEAR(R_from_quat.b.y, R_expected.b.y, 1e-4f);
301+
EXPECT_NEAR(R_from_quat.b.z, R_expected.b.z, 1e-4f);
302+
EXPECT_NEAR(R_from_quat.c.x, R_expected.c.x, 1e-4f);
303+
EXPECT_NEAR(R_from_quat.c.y, R_expected.c.y, 1e-4f);
304+
EXPECT_NEAR(R_from_quat.c.z, R_expected.c.z, 1e-4f);
305+
}
306+
}
307+
}
308+
254309
AP_GTEST_MAIN()

0 commit comments

Comments
 (0)