fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 19:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

add functions to compute yaw (321 and 312 sequence)

Browse Source 
from quaternion and rotation matrix
This commit is contained in:
Kamil Ritz
2020-08-27 09:51:56 +10:00
committed by Paul Riseborough
parent 15afa8ae17
commit fdc86c247a
9 changed files with 60 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/utils.hpp
+10
View File
@@ -2,6 +2,9 @@
#pragma once
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
@@ -21,6 +24,13 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
bool shouldUse321RotationSequence(const matrix::Dcmf& R);
float getEuler321Yaw(const matrix::Quatf& q);
float getEuler321Yaw(const matrix::Dcmf& R);
float getEuler312Yaw(const matrix::Quatf& q);
float getEuler312Yaw(const matrix::Dcmf& R);
namespace ecl{
inline float powf(float x, int exp)
{