Yaw, Roll Pitch

You need to calculate that from the vehicle orientation matrix. Here's math used in CC:

orientation is TelemInfoV01::mOri matrix
public const byte RowX = 0;
public const byte RowY = 1;
public const byte RowZ = 2;

Yaw = (float)Math.Atan2(orientation[rFactor2Constants.RowZ].x, orientation[rFactor2Constants.RowZ].z),

Pitch = (float)Math.Atan2(-orientation[rFactor2Constants.RowY].z,
Math.Sqrt(orientation[rFactor2Constants.RowX].z * orientation[rFactor2Constants.RowX].z + orientation[rFactor2Constants.RowZ].z * orientation[rFactor2Constants.RowZ].z)),

Roll = (float)Math.Atan2(orientation[rFactor2Constants.RowY].x,
Math.Sqrt(orientation[rFactor2Constants.RowX].x * orientation[rFactor2Constants.RowX].x + orientation[rFactor2Constants.RowZ].x * orientation[rFactor2Constants.RowZ].x))
 
Back
Top