Friends who know or want to develop UAVs will not be able to solve this problem. It is not difficult to spend time to understand their principles. Here are two principles for your reference:
Understanding of quaternion rotation
Four rotor attitude calculation principle
In the aspect of code implementation, I have written the attitude algorithm module for you to learn and reference.

Download address:
Baidu cloud link: https://pan.baidu.com/s/1kfeqp wyllgxdju6ejwejg extraction code: yawh
 Github link: https://github.com/diceTZ/Pose.git

Module input:
1. Acceleration 3-axis data
2. Gyro 3-axis data
3. Magnetometer 3-axis data (optional)
Module output:
1. Attitude angle (pit, roll, yaw)
2. Rotation matrix
3. Acceleration in world coordinate system
4. Corrected acceleration (horizontal acceleration and vertical acceleration), gyroscope
Note: the module coordinate is the right-hand coordinate system, the nose direction is the positive x-axis direction, and the sky is the positive z-axis direction.

Acceleration direction Description:
Accelerating towards the nose, ACC ű x is positive;
Accelerate to the left side of the nose, acc_y is positive;
The acceleration towards the sky, ACC ﹤ Z is greater than - 980cm / S ^ 2, for example: - 970cm / S ^ 2;

Gyro direction Description:
The thumb points to the nose, and the right four fingers rotate in the positive direction of gyro ﹐ X;
Other similarities

Post. H (post. C in download address)

//@By:
//@Prepared on: April 6, 2019
//@Revised on: April 6, 2019
//@File name: post. H
//@Description: attitude calculation module

#ifndef _POSE_H
#define _POSE_H

#ifndef u8 
#define u8 unsigned char
#endif

#ifndef s8 
#define s8 char
#endif

#ifndef XYZ_Data
#define XYZ_Data
typedef struct XYZ_Data_f
{
	float x;
	float y;	
	float z;
}XYZ_Data_f;

typedef struct XYZ_Data_s32
{
	long x;
	long y;
	long z;
}XYZ_Data_s32;

typedef struct XYZ_Data_s16
{
	short x;
	short y;
	short z;
}XYZ_Data_s16;

typedef struct XYZ_Data_s8
{
	s8 x;
	s8 y;	
	s8 z;
}XYZ_Data_s8;
#endif
typedef struct Pose_Flag
{
	u8 run;
	u8 use_mag;
}Pose_Flag;

typedef struct Pose_DInterface
{
	float *a_x;
	float *a_y;
	float *a_z;
	
	float *g_x;
	float *g_y;
	float *g_z;
	
	float *m_x;
	float *m_y;
	float *m_z;
}Pose_DInterface;

typedef struct Pose_Interface
{
	Pose_DInterface data;
}Pose_Interface;

typedef struct Pose_Data
{
	float yaw;
	float rol;
	float pit;
	 
	float rotate_matrix[3][3]; //Rotation matrix
	 
	XYZ_Data_f acc_world;    //Acceleration in world coordinate system
	XYZ_Data_f mag_world;    //The intensity of magnetic field in the world coordinate system
	 
	XYZ_Data_f acc_correct;   //Acceleration in body coordinate system -- corrected acceleration after pitching and rolling
	XYZ_Data_f mag_correct;   //The intensity of magnetic field in the body coordinate system -- corrected the intensity of magnetic field after pitching and rolling
	XYZ_Data_f gyro_correct;  //Integrating accelerometer and magnetometer data, corrected gyroscope value
}Pose_Data;

typedef struct Pose_Process
{
	float mag_yaw;       //Yaw angle of magnetometer
	float mag_yaw_bias;     //Deviation of yaw angle corrected by magnetometer
	float quaternion[4];    //Four yuan number
	XYZ_Data_f error;      //Deviation between accelerometer and equivalent gravity
	XYZ_Data_f error_integral; //Error integral
}Pose_Process;

typedef struct Pose_Parameter
{
	float correct_kp;	//Correction speed
	float error_kp;		//Correction speed
	float error_ki;
}Pose_Parameter;

typedef struct Pose_Module
{
	Pose_Flag flag;
	Pose_Interface interface; 
	Pose_Process process;
	Pose_Data data;
	Pose_Parameter parameter;
}Pose_Module;

//Initialize structure
void initPose_Module(Pose_Module *pose);
//Computational attitude
void calculatePose_Module(Pose_Module *pose, float cycle);

Routine code:

#include "stdlib.h"
#include "pose.h"

Pose_Module pose;

int main(void)
{
	float acc_x = 0, acc_y = 0, acc_z = -980.0f;
	float gyro_x = 0, gyro_y = 0, gyro_z = 0;
	float pit, rol, yaw;
	
	//Initialize structure
	initPose_Module(&pose);
	//Connection interface
	pose.interface.data.a_x = &acc_x;
	pose.interface.data.a_y = &acc_y;
	pose.interface.data.a_z = &acc_z;
	pose.interface.data.g_x = &gyro_x;
	pose.interface.data.g_y = &gyro_y;
	pose.interface.data.g_z = &gyro_z;
	
	while(1)
	{
	 	//Algorithm module of calculating attitude
 		calculatePose_Module(&pose, 0.01f);
 
 		//get data
 		pit = pose.data.pit;
 		rol = pose.data.rol;
		yaw = pose.data.yaw;
	}
}