dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mathlib: utilities refactor float to function template (for optional double precision usage) 

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2024-03-20 11:10:37 -04:00 committed by GitHub
parent 62b8db153b
commit 35532609c9
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 57 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
src/lib/mathlib/math/Utilities.hpp
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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(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) // rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
// See http://www.atacolorado.com/eulersequences.doc // See http://www.atacolorado.com/eulersequences.doc
inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) template<typename T = float>
inline matrix::Dcm<T> taitBryan312ToRotMat(const matrix::Vector3<T> &rot312)
{ {
// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence // 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 T c2 = cosf(rot312(2)); // third rotation is pitch
const float s2 = sinf(rot312(2)); const T s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1)); // second rotation is roll const T s1 = sinf(rot312(1)); // second rotation is roll
const float c1 = cosf(rot312(1)); const T c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0)); // first rotation is yaw const T s0 = sinf(rot312(0)); // first rotation is yaw
const float c0 = cosf(rot312(0)); const T c0 = cosf(rot312(0));
matrix::Dcmf R; matrix::Dcm<T> R;
R(0, 0) = c0 * c2 - s0 * s1 * s2; R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1; R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1; R(2, 2) = c2 * c1;
@ -75,20 +76,21 @@ inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
return R; return R;
} }
inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) template<typename T = float>
inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &quat)
{ {
const float q00 = quat(0) * quat(0); const T q00 = quat(0) * quat(0);
const float q11 = quat(1) * quat(1); const T q11 = quat(1) * quat(1);
const float q22 = quat(2) * quat(2); const T q22 = quat(2) * quat(2);
const float q33 = quat(3) * quat(3); const T q33 = quat(3) * quat(3);
const float q01 = quat(0) * quat(1); const T q01 = quat(0) * quat(1);
const float q02 = quat(0) * quat(2); const T q02 = quat(0) * quat(2);
const float q03 = quat(0) * quat(3); const T q03 = quat(0) * quat(3);
const float q12 = quat(1) * quat(2); const T q12 = quat(1) * quat(2);
const float q13 = quat(1) * quat(3); const T q13 = quat(1) * quat(3);
const float q23 = quat(2) * quat(3); const T q23 = quat(2) * quat(3);
matrix::Dcmf dcm; matrix::Dcm<T> dcm;
dcm(0, 0) = q00 + q11 - q22 - q33; dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33; dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = 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 // 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 roll than pitch tilt and a 3-1-2 rotation sequence
// when there is more pitch than roll tilt to avoid gimbal lock. // when there is more pitch than roll tilt to avoid gimbal lock.
inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) template<typename T = float>
inline bool shouldUse321RotationSequence(const matrix::Dcm<T> &R)
{ {
return fabsf(R(2, 0)) < fabsf(R(2, 1)); return fabsf(R(2, 0)) < fabsf(R(2, 1));
} }
inline float getEuler321Yaw(const matrix::Dcmf &R) template<typename T = float>
inline float getEuler321Yaw(const matrix::Dcm<T> &R)
{ {
return atan2f(R(1, 0), R(0, 0)); return atan2f(R(1, 0), R(0, 0));
} }
inline float getEuler312Yaw(const matrix::Dcmf &R) template<typename T = float>
inline float getEuler312Yaw(const matrix::Dcm<T> &R)
{ {
return atan2f(-R(0, 1), R(1, 1)); return atan2f(-R(0, 1), R(1, 1));
} }
inline float getEuler321Yaw(const matrix::Quatf &q) template<typename T = float>
inline T getEuler321Yaw(const matrix::Quaternion<T> &q)
{ {
// Values from yaw_input_321.c file produced by // Values from yaw_input_321.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m // 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 T a = static_cast<T>(2.) * (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 b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
return atan2f(a, b); return atan2f(a, b);
} }
inline float getEuler312Yaw(const matrix::Quatf &q) template<typename T = float>
inline T getEuler312Yaw(const matrix::Quaternion<T> &q)
{ {
// Values from yaw_input_312.c file produced by // Values from yaw_input_312.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m // 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 T a = static_cast<T>(2.) * (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 b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
return atan2f(a, b); return atan2f(a, b);
} }
inline float getEulerYaw(const matrix::Dcmf &R) template<typename T = float>
inline T getEulerYaw(const matrix::Dcm<T> &R)
{ {
if (shouldUse321RotationSequence(R)) { if (shouldUse321RotationSequence(R)) {
return getEuler321Yaw(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<typename T = float>
inline T getEulerYaw(const matrix::Quaternion<T> &q)
{ {
matrix::Eulerf euler321(rot_in); return getEulerYaw(matrix::Dcm<T>(q));
euler321(2) = yaw;
return matrix::Dcmf(euler321);
} }
inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) template<typename T = float>
inline matrix::Dcm<T> updateEuler321YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
{ {
const matrix::Vector3f rotVec312(yaw, // yaw matrix::Euler<T> euler321(rot_in);
asinf(rot_in(2, 1)), // roll euler321(2) = yaw;
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch return matrix::Dcm<T>(euler321);
}
template<typename T = float>
inline matrix::Dcm<T> updateEuler312YawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
{
const matrix::Vector3<T> rotVec312(yaw, // yaw
asinf(rot_in(2, 1)), // roll
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
return taitBryan312ToRotMat(rotVec312); return taitBryan312ToRotMat(rotVec312);
} }
// Checks which euler rotation sequence to use and update yaw in rotation matrix // 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<typename T = float>
inline matrix::Dcm<T> updateYawInRotMat(T yaw, const matrix::Dcm<T> &rot_in)
{ {
if (shouldUse321RotationSequence(rot_in)) { if (shouldUse321RotationSequence(rot_in)) {
return updateEuler321YawInRotMat(yaw, rot_in); return updateEuler321YawInRotMat(yaw, rot_in);