Attitude solver algorithm source code module --C

Understand or want to develop UAVs friends around, but certainly this crop attitude solution, take the time to understand their principle is not difficult, provided here for your reference links two principles:
quaternion rotation of understanding
four-rotor Attitude calculation principle
in terms of code to achieve, I've written a gesture solver algorithm module for study and reference.

下载地址：
百度云链接：https://pan.baidu.com/s/1kFeqP_WYllgXdjU6Ejwejg 提取码：yawh
Github链接：https://github.com/diceTZ/Pose.git

Input module:
1, 3-axis acceleration data
2, data 3-axis gyroscope
3, 3-axis magnetometer data (optional)
Output module:
an attitude angle (PIT, roll, Yaw)
2, rotation matrix
3, World acceleration of the coordinate system
4, an acceleration (horizontal acceleration and vertical acceleration) of the corrected gyro
Note: right-handed coordinate module coordinates, x-axis positive direction is the head direction, the z-axis positive direction the sky.

Description direction acceleration:
accelerating the carriage direction, acc_x is positive;
accelerated toward the left side head direction, acc_y is positive;
acceleration skyward direction, acc_z greater than -980 cm / s ^ 2 For example: -970 cm / s ^ 2;

Description gyroscope direction:
the thumb points in the direction of the head, right hand four fingers as a positive direction of rotation gyro_x;
Other Similarly

pose.h (pose.c download address)

//@作者   ：tou_zi
//@编写时间 ：2019年4月6日
//@修改时间 ：2019年4月6日
//@文件名  ：pose.h
//@描述   ：姿态解算模块

#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]; //旋转矩阵
	 
	XYZ_Data_f acc_world;    //世界坐标系下的加速度
	XYZ_Data_f mag_world;    //世界坐标系下的磁场强度 -- 只与无人机位置有关的量
	 
	XYZ_Data_f acc_correct;   //机体坐标系下的加速度 -- 矫正了俯仰翻滚后的加速度
	XYZ_Data_f mag_correct;   //机体坐标系下的磁场强度 -- 矫正了俯仰翻滚后的磁场强度
	XYZ_Data_f gyro_correct;  //融合加速度和磁力计数据，矫正后的陀螺仪值
}Pose_Data;

typedef struct Pose_Process
{
	float mag_yaw;       //磁力计的偏航角
	float mag_yaw_bias;     //磁力计矫正的偏航角偏差
	float quaternion[4];    //四元数
	XYZ_Data_f error;      //由加速度计与等效重力的偏差
	XYZ_Data_f error_integral; //误差积分
}Pose_Process;

typedef struct Pose_Parameter
{
	float correct_kp;	//矫正速度
	float error_kp;		//矫正速度
	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;

//初始化结构体
void initPose_Module(Pose_Module *pose);
//计算姿态
void calculatePose_Module(Pose_Module *pose, float cycle);

Sample 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;
	
	//初始化结构体
	initPose_Module(&pose);
	//连接接口
	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)
	{
	 	//运算姿态解算算法模块
 		calculatePose_Module(&pose, 0.01f);
 
 		//获取数据
 		pit = pose.data.pit;
 		rol = pose.data.rol;
		yaw = pose.data.yaw;
	}
}