forked from Archive/PX4-Autopilot
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
GitHub
parent
62b8db153b
commit
35532609c9
|
|
@ -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<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
|
||||
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<T> 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<typename T = float>
|
||||
inline matrix::Dcm<T> quatToInverseRotMat(const matrix::Quaternion<T> &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<T> 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<typename T = float>
|
||||
inline bool shouldUse321RotationSequence(const matrix::Dcm<T> &R)
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
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
|
||||
// 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<T>(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<typename T = float>
|
||||
inline T getEuler312Yaw(const matrix::Quaternion<T> &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<T>(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<typename T = float>
|
||||
inline T getEulerYaw(const matrix::Dcm<T> &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<typename T = float>
|
||||
inline T getEulerYaw(const matrix::Quaternion<T> &q)
|
||||
{
|
||||
matrix::Eulerf euler321(rot_in);
|
||||
euler321(2) = yaw;
|
||||
return matrix::Dcmf(euler321);
|
||||
return getEulerYaw(matrix::Dcm<T>(q));
|
||||
}
|
||||
|
||||
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
|
||||
asinf(rot_in(2, 1)), // roll
|
||||
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
|
||||
matrix::Euler<T> euler321(rot_in);
|
||||
euler321(2) = yaw;
|
||||
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);
|
||||
}
|
||||
|
||||
// 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)) {
|
||||
return updateEuler321YawInRotMat(yaw, rot_in);
|
||||
|
|
|
|||
Loading…
Reference in New Issue