mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 02:24:06 +08:00
61 lines
1.6 KiB
C++
61 lines
1.6 KiB
C++
#include "utils.hpp"
|
|
|
|
matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &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));
|
|
|
|
matrix::Dcmf R;
|
|
R(0, 0) = c0 * c2 - s0 * s1 * s2;
|
|
R(1, 1) = c0 * c1;
|
|
R(2, 2) = c2 * c1;
|
|
R(0, 1) = -c1 * s0;
|
|
R(0, 2) = s2 * c0 + c2 * s1 * s0;
|
|
R(1, 0) = c2 * s0 + s2 * s1 * c0;
|
|
R(1, 2) = s0 * s2 - s1 * c0 * c2;
|
|
R(2, 0) = -s2 * c1;
|
|
R(2, 1) = s1;
|
|
|
|
return R;
|
|
}
|
|
|
|
float kahanSummation(float sum_previous, float input, float &accumulator)
|
|
{
|
|
float y = input - accumulator;
|
|
float t = sum_previous + y;
|
|
accumulator = (t - sum_previous) - y;
|
|
return t;
|
|
}
|
|
|
|
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
|
|
{
|
|
float q00 = quat(0) * quat(0);
|
|
float q11 = quat(1) * quat(1);
|
|
float q22 = quat(2) * quat(2);
|
|
float q33 = quat(3) * quat(3);
|
|
float q01 = quat(0) * quat(1);
|
|
float q02 = quat(0) * quat(2);
|
|
float q03 = quat(0) * quat(3);
|
|
float q12 = quat(1) * quat(2);
|
|
float q13 = quat(1) * quat(3);
|
|
float q23 = quat(2) * quat(3);
|
|
|
|
matrix::Dcmf dcm;
|
|
dcm(0, 0) = q00 + q11 - q22 - q33;
|
|
dcm(1, 1) = q00 - q11 + q22 - q33;
|
|
dcm(2, 2) = q00 - q11 - q22 + q33;
|
|
dcm(1, 0) = 2.0f * (q12 - q03);
|
|
dcm(2, 0) = 2.0f * (q13 + q02);
|
|
dcm(0, 1) = 2.0f * (q12 + q03);
|
|
dcm(2, 1) = 2.0f * (q23 - q01);
|
|
dcm(0, 2) = 2.0f * (q13 - q02);
|
|
dcm(1, 2) = 2.0f * (q23 + q01);
|
|
|
|
return dcm;
|
|
}
|