我把这个文件的所有代码贴上来了,供大家参考,由于本人水平有限,且匿名代码注释比较少,所以很多也不是很懂,实在是一些莫名的定义太多了,什么w/x/y/z/h之类的,只求先能看懂大概逻辑,至于一些细节日后再啃
先来一个融合磁力计的Mahony互补滤波算法
https://blog.csdn.net/zhiyu_buliang/article/details/89056519
有心的人会发现匿名现在的代码也不过是根据原来经典Mahony代码改过来的,大致意思都在,仔细对比下看看。
本人在匿名代码的基础上增加了一些注释,只是个人的见解,免不了会有很多错误,希望大家多多指正。
/******************** (C) COPYRIGHT 2016 ANO Tech ***************************
* 作者 :匿名科创
* 文件名 :ANO_IMU.c
* 描述 :姿态解算函数
* 官网 :www.anotc.com
* 淘宝 :anotc.taobao.com
* 技术Q群 :190169595
*****************************************************************************/
#include "Ano_Imu.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "Ano_DT.h"
//#include "ANO_RC.h"
/*参考坐标,定义为ANO坐标 ---> 西北天
俯视,机头方向为x正方向
+x
|
+y--|--
|
*/
//涉及磁力计的XY二维变换
//原谅我没看懂到底是干嘛的
void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_XYZ])
{
h[X] = w[X] * ref_ax[X] + w[Y] *ref_ax[Y];
h[Y] = w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];
}
void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{
w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
w[Y] = h[X] *ref_ax[Y] + h[Y] * ref_ax[X];
}
//没看懂
float mag_yaw_calculate(float dT,float mag_val[VEC_XYZ],float g_z_vec[VEC_XYZ],float h_mag_val[VEC_XYZ])//
{
// float mag_h_norm;
// float mag_2d_vec[VEC_XYZ];
vec_3dh_transition(g_z_vec, mag_val, h_mag_val);
// mag_h_norm = my_sqrt(my_pow(h_mag_val[X]) + my_pow(h_mag_val[Y]));
//
// mag_2d_vec[X] = safe_div(h_mag_val[X],mag_h_norm,0);
// mag_2d_vec[Y] = safe_div(h_mag_val[Y],mag_h_norm,0);
// return (fast_atan2(mag_2d_vec[Y], mag_2d_vec[X]) *57.3f) ;//
return (fast_atan2(h_mag_val[Y], h_mag_val[X]) *57.3f) ;//
}
#define USE_MAG
#define USE_LENGTH_LIM
//imu最长的结构体定义变量赋初值
_imu_st imu_data = {1,0,0,0,
{0,0,0},