姿态解算算法模块--C源码

了解或想开发无人机的朋友肯定绕不过姿态解算这茬，花点时间去了解它们原理并不难，这里提供两个原理链接供大家参考：
四元数表示旋转的理解
四旋翼姿态解算原理
而在代码实现方面，我这里写好了姿态解算算法模块供大家学习和参考。

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

模块的输入：
1、加速度3轴数据
2、陀螺仪3轴数据
3、磁力计3轴数据（可选）
模块的输出：
1、姿态角（pit，roll， yaw）
2、旋转矩阵
3、世界坐标系下的加速度
4、矫正后的加速度（水平加速度和垂直加速度），陀螺仪
注：模块坐标为右手坐标系，机头方向为x轴正方向，天空为z轴正方向。

加速度方向说明：
向机头方向加速，acc_x为正；
向机头左侧方向加速，acc_y为正；
向天空方向加速度，acc_z大于-980 cm/s^2 例如：-970 cm/s^2；

陀螺仪方向说明：
拇指指向机头方向，右手四指旋向为gyro_x正方向；
其他同理

pose.h（pose.c在下载地址中）

//@作者   ：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);

例程代码:

#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;
	}
}