From 35532609c9f8b337372c57d12f12e24aa5634738 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Mar 2024 11:10:37 -0400 Subject: [PATCH] mathlib: utilities refactor float to function template (for optional double precision usage) Co-authored-by: Mathieu Bresciani --- src/lib/mathlib/math/Utilities.hpp | 97 ++++++++++++++++++------------ 1 file changed, 57 insertions(+), 40 deletions(-) diff --git a/src/lib/mathlib/math/Utilities.hpp b/src/lib/mathlib/math/Utilities.hpp index 9b8f7047c4..890346b973 100644 --- a/src/lib/mathlib/math/Utilities.hpp +++ b/src/lib/mathlib/math/Utilities.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,17 +51,18 @@ static constexpr float sq(float var) { return var * var; } // rot312(1) - Second rotation is a RH rotation about the X axis (rad) // rot312(2) - Third rotation is a RH rotation about the Y axis (rad) // See http://www.atacolorado.com/eulersequences.doc -inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) +template +inline matrix::Dcm taitBryan312ToRotMat(const matrix::Vector3 &rot312) { // Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence - const float c2 = cosf(rot312(2)); // third rotation is pitch - const float s2 = sinf(rot312(2)); - const float s1 = sinf(rot312(1)); // second rotation is roll - const float c1 = cosf(rot312(1)); - const float s0 = sinf(rot312(0)); // first rotation is yaw - const float c0 = cosf(rot312(0)); + const T c2 = cosf(rot312(2)); // third rotation is pitch + const T s2 = sinf(rot312(2)); + const T s1 = sinf(rot312(1)); // second rotation is roll + const T c1 = cosf(rot312(1)); + const T s0 = sinf(rot312(0)); // first rotation is yaw + const T c0 = cosf(rot312(0)); - matrix::Dcmf R; + matrix::Dcm R; R(0, 0) = c0 * c2 - s0 * s1 * s2; R(1, 1) = c0 * c1; R(2, 2) = c2 * c1; @@ -75,20 +76,21 @@ inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) return R; } -inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) +template +inline matrix::Dcm quatToInverseRotMat(const matrix::Quaternion &quat) { - const float q00 = quat(0) * quat(0); - const float q11 = quat(1) * quat(1); - const float q22 = quat(2) * quat(2); - const float q33 = quat(3) * quat(3); - const float q01 = quat(0) * quat(1); - const float q02 = quat(0) * quat(2); - const float q03 = quat(0) * quat(3); - const float q12 = quat(1) * quat(2); - const float q13 = quat(1) * quat(3); - const float q23 = quat(2) * quat(3); + const T q00 = quat(0) * quat(0); + const T q11 = quat(1) * quat(1); + const T q22 = quat(2) * quat(2); + const T q33 = quat(3) * quat(3); + const T q01 = quat(0) * quat(1); + const T q02 = quat(0) * quat(2); + const T q03 = quat(0) * quat(3); + const T q12 = quat(1) * quat(2); + const T q13 = quat(1) * quat(3); + const T q23 = quat(2) * quat(3); - matrix::Dcmf dcm; + matrix::Dcm dcm; dcm(0, 0) = q00 + q11 - q22 - q33; dcm(1, 1) = q00 - q11 + q22 - q33; dcm(2, 2) = q00 - q11 - q22 + q33; @@ -105,40 +107,46 @@ inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) // We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence // when there is more roll than pitch tilt and a 3-1-2 rotation sequence // when there is more pitch than roll tilt to avoid gimbal lock. -inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) +template +inline bool shouldUse321RotationSequence(const matrix::Dcm &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); } -inline float getEuler321Yaw(const matrix::Dcmf &R) +template +inline float getEuler321Yaw(const matrix::Dcm &R) { return atan2f(R(1, 0), R(0, 0)); } -inline float getEuler312Yaw(const matrix::Dcmf &R) +template +inline float getEuler312Yaw(const matrix::Dcm &R) { return atan2f(-R(0, 1), R(1, 1)); } -inline float getEuler321Yaw(const matrix::Quatf &q) +template +inline T getEuler321Yaw(const matrix::Quaternion &q) { // Values from yaw_input_321.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - const float a = 2.f * (q(0) * q(3) + q(1) * q(2)); - const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) + q(1) * q(2)); + const T b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEuler312Yaw(const matrix::Quatf &q) +template +inline T getEuler312Yaw(const matrix::Quaternion &q) { // Values from yaw_input_312.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - const float a = 2.f * (q(0) * q(3) - q(1) * q(2)); - const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) - q(1) * q(2)); + const T b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEulerYaw(const matrix::Dcmf &R) +template +inline T getEulerYaw(const matrix::Dcm &R) { if (shouldUse321RotationSequence(R)) { return getEuler321Yaw(R); @@ -148,23 +156,32 @@ inline float getEulerYaw(const matrix::Dcmf &R) } } -inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline T getEulerYaw(const matrix::Quaternion &q) { - matrix::Eulerf euler321(rot_in); - euler321(2) = yaw; - return matrix::Dcmf(euler321); + return getEulerYaw(matrix::Dcm(q)); } -inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateEuler321YawInRotMat(T yaw, const matrix::Dcm &rot_in) { - const matrix::Vector3f rotVec312(yaw, // yaw - asinf(rot_in(2, 1)), // roll - atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch + matrix::Euler euler321(rot_in); + euler321(2) = yaw; + return matrix::Dcm(euler321); +} + +template +inline matrix::Dcm updateEuler312YawInRotMat(T yaw, const matrix::Dcm &rot_in) +{ + const matrix::Vector3 rotVec312(yaw, // yaw + asinf(rot_in(2, 1)), // roll + atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch return taitBryan312ToRotMat(rotVec312); } // Checks which euler rotation sequence to use and update yaw in rotation matrix -inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateYawInRotMat(T yaw, const matrix::Dcm &rot_in) { if (shouldUse321RotationSequence(rot_in)) { return updateEuler321YawInRotMat(yaw, rot_in);