陀螺仪c语言算法,最简单的航姿仪算法C程序（AHRS）

#include "math.h"

// 最简单的航姿仪Mahony算法，所谓“最简单”就意味着在加速机动环境下效果会不太好，没做任何容错处理。

// 注意: 传感器/机体坐标系(b):x右-y前-z上; 导航坐标系(n):E东-N北-U天

// gx,gy,gz陀螺仪采样(deg/s), ax,ay,az加计采样(可为任意单位或归一化单位)

// mx,my,mz磁采样(可为任意单位或归一化单位)，若三轴磁均输入零则表示不利用磁信息

// 根据网上程序整理优化 by Yan Gongmin, 2017-06-07

void MahonyInit(float tau);

void MahonyUpdate(float gx, float gy, float gz, float ax, float ay,

float az, float mx, float my, float mz, float ts);

float MahonyGetEuler(float *pitch, float *roll, float *yaw);

void MahonyGetDrift(float *driftx, float *drifty, float

*driftz);

#define DEG (3.1415926f/180.0f)

#define DPS (DEG/1.0f)

#define DPH (DEG/3600.0f)

static float q0,q1,q2,q3, exInt,eyInt,ezInt, tk;

static float C00,C01,C02, C10,C11,C12, C20,C21,C22;

static float Kp, Ki;

static void qua2cnb(void);