Euler angle and four-dollar conversion--programming

Three or four yuan to Euler angle conversion

As a result of arctan and arcsin, this does not cover all orientations (the range of values for the corners has been met), so atan2 is needed instead of arctan.

Iv. use in other coordinate systems
In other coordinate systems, the above formula should be adjusted according to the definition of the axis. As in Direct3D, the x-axis of the Cartesian coordinate system changes to the z-axis, the y-axis to the x-axis, and the z-axis to the y-axis (regardless of direction).

2.2.34-dollar calculation of the design of Euler's angle
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; Defining four elements
void Imu_update (void)
{
float norm;
Float GX = Mpu6500_gyro.x*gyro_gr,gy = MPU6500_GYRO.Y*GYRO_GR,GZ = unit conversion between mpu6500_gyro.z*gyro_gr;//angles
float ax = Acc_avg.x,ay = Acc_avg.y,az = acc_avg.z;
float q0q0 = q0 * Q0;
float q0q1 = q0 * Q1;
float Q0Q2 = q0 * Q2;
float Q1Q1 = Q1 * Q1;
float Q1Q3 = Q1 * Q3;
float q2q2 = q2* Q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
Float VX, vy, VZ;
Float ex, EY, EZ;
Floatq0_yawq0_yaw = Q0_yaw * Q0_YAW;
Floatq1_yawq1_yaw = Q1_yaw * Q1_YAW;
Floatq2_yawq2_yaw = Q2_yaw * Q2_YAW;
Floatq3_yawq3_yaw = Q3_yaw * Q3_YAW;
Floatq1_yawq2_yaw = Q1_yaw * Q2_YAW;
Floatq0_yawq3_yaw = Q0_yaw * Q3_YAW;

Yaw axis calculation ******************************

Differential equations for yaw axis four elements
Q0_yaw = Q0_yaw + (-q1_yaw * Gx-q2_yaw * Gy-q3_yaw * gz) * sample_half_t;
Q1_yaw = Q1_yaw + (Q0_yaw * GX + q2_yaw * Gz-q3_yaw * gy) * sample_half_t;
Q2_yaw = Q2_yaw + (Q0_yaw * Gy-q1_yaw * GZ + q3_yaw * GX) * sample_half_t;
Q3_yaw = Q3_yaw + (Q0_yaw * gz + q1_yaw * Gy-q2_yaw * GX) * sample_half_t;
Normalized Yaw axis four yuan
Norm = sqrt (Q0_yawq0_yaw + q1_yawq1_yaw + Q2_yawq2_yaw + q3_yawq3_yaw);
Q0_yaw = Q0_yaw/norm;
Q1_yaw = Q1_yaw/norm;
Q2_yaw = Q2_yaw/norm;
Q3_yaw = Q3_yaw/norm;
if (ax * ay * az== 0)
return;
Normalized Accelerometer values
Norm = sqrt (ax * ax + ay * ay + az * az);
Ax = ax/norm;
ay = ay/norm;
AZ = az/norm;
Estimating gravity direction and flow/change
VX = 2 * (Q1Q3-Q0Q2);
VY = 2 * (q0q1 + q2q3);
VZ = q0q0-q1q1-q2q2 + q3q3;
The difference between the outer product of the vector and the subtraction is the error
ex = (AY * vz-az * vy);
EY = (AZ * vx-ax * vz);
EZ = (AX * vy-ay * VX);
PI Calculation of error
Ex_int = Ex_int + ex * IMU_KI;
Ey_int = ey_int + ey * IMU_KI;
Ez_int = Ez_int + ez * IMU_KI;

Calibration Gyroscope
GX = GX + IMU_KP * ex + ex_int;
GY = gy + imu_kp * ey + ey_int;
GZ = GZ + IMU_KP * ez + ez_int;

The differential equation of four elements
Q0 = q0 + (-Q1 * gx-q2*gy-q3*gz) *sample_half_t;
Q1 = Q1 + (Q0*GX + q2*gz-q3*gy) *sample_half_t;
Q2 = q2 + (Q0*gy-q1*gz + q3*gx) *sample_half_t;
Q3 = Q3 + (Q0*gz + q1*gy-q2*gx) *sample_half_t;

Normalized pitch, roll axis four yuan
Norm = sqrt (q0q0 + q1q1 + q2q2 + q3q3);
q0 = Q0/norm;
q1 = q1/norm;
q2 = Q2/norm;
Q3 = Q3/norm;

Solving Euler angles
angle.x = atan2 (2 * q2q3 + 2 * q0q1,-2 * q1q1-2 * Q2Q2 + 1) * 57.3F;
ANGLE.Y = ASIN ( -2 * q1q3 + 2 * q0q2) * 57.3F;
Angle.z = atan2 (2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw,-2 * q2_yawq2_yaw-2 * Q3_yawq3_yaw + 1) * 57.3F;
}
Program Description: The program is provided by the open source quad-axis BlackHole1.
2.2.4 Hardware acceleration Engine (DMP) theory analysis
The MPU-6050 incorporates a 3-axis gyroscope, 3-axis accelerator, and Digital motion processing (dmp:digital motion Processor) hardware acceleration engine, the most powerful feature of which is its use of the MPU6050 integrated internal acceleration engine, Avoid the above using MPU6050 raw data, through a variety of algorithms to obtain a stable four, and we use the official DMP driver directly read out the value of four yuan, save a lot of work, in the use of four-dollar formula to calculate a stable, effective Euler angle. However, because the geomagnetic compensation is not used, the yaw angle (YAW) will produce an error over time.
Some of the procedures for calculating Euler angles are as follows:
q0 = quat[0]/q30;//q30 format converted to floating-point number
Q1 = quat[1]/q30;
Q2 = quat[2]/q30;
Q3 = quat[3]/q30;
Calculate pitch angle/roll angle/Heading angle
*pitch = ASIN ( -2 * Q1 * q3 + 2 * q0* Q2) * 57.3;//pitch
*roll = atan2 (2 * Q2 * q3 + 2 * q0 * Q1,-2 * Q1 * q1-2 * q2* Q2 + 1) * 57.3;//Roll
*yaw = atan2 (q1*q2 + q0*q3), q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw

Euler angle and four-dollar conversion--programming