匿名飞控STM32版代码整理之Ano

一、ANO_Imu.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”

//世界坐标平面XY转平面航向坐标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];

}
//平面航向坐标XY转世界坐标平面XY
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];

}

//载体坐标转世界坐标（ANO约定等同与地理坐标）
float att_matrix[3][3]; //必须由姿态解算算出该矩阵
void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])
{
for(u8 i = 0;i<3;i++)
{
float temp = 0;
for(u8 j = 0;j<3;j++)
{

         temp += a[j] *att_matrix[i][j];}w[i] = temp;}

}

//float mag_yaw_calculate(float dT,float mag_val[VEC_XYZ],float g_z_vec[VEC_XYZ],float h_mag_val[VEC_XYZ])//
//{

// vec_3dh_transition(g_z_vec, mag_val, h_mag_val);

// return (fast_atan2(h_mag_val[Y], h_mag_val[X]) *57.3f) ;//
//}

#define USE_MAG
#define USE_LENGTH_LIM

static float vec_err[VEC_XYZ];
static float vec_err_i[VEC_XYZ];
static float q0q1,q0q2,q1q1,q1q3,q2q2,q2q3,q3q3,q1q2,q0q3;//q0q0,
static float mag_yaw_err,mag_err_dot_prudoct,mag_val_f[VEC_XYZ];
static float imu_reset_val;

static u16 reset_cnt;

_imu_st imu_data = {1,0,0,0,//单位四元数
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
{0,0,0},
0,0,0};

_imu_state_st imu_state = {1,1,1,1,1,1,1,1};//状态结构体：陀螺仪的p、i,磁力计的p obs_en等等

static float mag_2d_w_vec[2][2] = {{1,0},{1,0}};//地理坐标中，水平面磁场方向恒为南北 (1,0) mag_2d_w_vec[0][0]={1,0} mag_2d_w_vec[0][1]={1,0}

float imu_test[3];

//在文件Ano_FlightDataCal.c被调用

//惯性测量单元数据更新(惯性测量单元：陀螺仪、加速度计、磁力计)

//dT:采样周期 *state:imu(惯性测量单元)状态结构体指针(实际传入的是&imu_state)
//gyr[VEC_XYZ]：陀螺仪xyz方向的数据 acc[VEC_XYZ]：加速度计xyz方向的数据
//mag_val[VEC_XYZ]：磁力计xyz方向的数据 *imu:imu结构体指针(实际传入的是&imu_data)

//姿态更新
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
// const float kp = 0.2f,ki = 0.001f;
// const float kmp = 0.1f;

static float kp_use = 0,ki_use = 0,mkp_use = 0;float acc_norm_l,acc_norm_l_recip,q_norm_l;float acc_norm[VEC_XYZ];float d_angle[VEC_XYZ];

// q0q0 = imu->w * imu->w;

 q0q1 = imu->w * imu->x;q0q2 = imu->w * imu->y;q1q1 = imu->x * imu->x;q1q3 = imu->x * imu->z;q2q2 = imu->y * imu->y;q2q3 = imu->y * imu->z;q3q3 = imu->z * imu->z;q1q2 = imu->x * imu->y;q0q3 = imu->w * imu->z;//obs_en始终为0---------------------obs机体-----------------------------------------------------if(state->obs_en){//计算机体坐标下的运动加速度观测量。坐标系为北西天for(u8 i = 0;i<3;i++){s32 temp = 0;for(u8 j = 0;j<3;j++){temp += imu->obs_acc_w[j] * att_matrix[j][i];//t[i][j] 转置为 t[j][i]}imu->obs_acc_a[i] = temp;//机体坐标系下的IMU  acc_a[i]的值imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i]; // 地理坐标系下的 acc[i]=加速度计测量的值 - 机体坐标系下的值}}//只执行下面的----------------------------------------------------------------------else{for(u8 i = 0;i<3;i++){         imu->gra_acc[i] = acc[i];//把加速度计的测量值保存在指针里面gra_acc//%%%地理坐标系下的加速度值等于加速度计测量的加速度值}}
//把加速度计的测量值平方和再开根号，再倒数，并保存acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));acc_norm_l = safe_div(1,acc_norm_l_recip,0);//再求倒数--->结果就是加速度计的测量值的平方和开根号// 加速度计的读数，单位化。for(u8 i = 0;i<3;i++){ acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;}// 载体坐标下的x方向向量，单位化---->  单位化的旋转矩阵Rcb 与向量（1 0 0）的转置  相乘的结果
att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;// 载体坐标下的y方向向量，单位----->单位化的旋转矩阵Rcb 与向量（0 1 0）的转置  相乘的结果
att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;// 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。---->单位化的旋转矩阵Rcb 与向量（0 0 1）的转置  相乘的结果
att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);//以上就把旋转矩阵求解出来了att_matrix[][]//水平面方向单位向量   将水平坐标系的x,y平方和，开根号 并求倒数   reciprocal：倒数的
float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;//*****************************************************************************************************// 计算载体坐标下的运动加速度。for(u8 i = 0;i<3;i++){imu->a_acc[i] = (s32)(acc[i] - 981 * imu->z_vec[i]);//将加速度计测量的数据减去(载体坐标系下)z轴方向向量//载体坐标系下的加速度 = 加速度计测量的加速度值 - 载体重力加速度在Z轴上的分量}//计算世界坐标下的运动加速度。坐标系为北西天-----------------原理就是：旋转矩阵(机体转世界)乘上机体下的加速度for(u8 i = 0;i<3;i++){s32 temp = 0;for(u8 j = 0;j<3;j++){temp += imu->a_acc[j] *att_matrix[i][j];//}imu->w_acc[i] = temp; //世界坐标系下的加速度值}w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);//将世界坐标系的运动加速度转换成水平面的运动加速度---------这是作甚么// >>>>>>>>>>>>>>> 加速度计读取的单位方向 与 重力加速度单位方向的差值 = 两个方向的叉乘<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
vec_err[X] =  (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);

#ifdef USE_MAG

 //电子罗盘赋值为float矢量for(u8 i = 0;i<3;i++){mag_val_f[i] = (float)mag_val[i];} if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))//当磁力计的三维坐标有值时，就开始对磁力计的数据进行处理{//把载体坐标下的罗盘数据转换到世界坐标系下a2w_3d_trans(mag_val_f,imu->w_mag);//将磁力计数据 平方和->开根号->倒数   float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));//将磁力计的数值归一化成方向向量   mag_2d_w_vec[1][0] = imu->w_mag[0] * l_re_tmp;//mag_2d_w_vec[1][1] = imu->w_mag[1] * l_re_tmp;//计算南北朝向方向向量的误差，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)//mag_2d_w_vec[1]:磁力计的测量数据归一化后的值-----实际值//mag_2d_w_vec[0]:(1,0)----------------------------理论值//两者叉乘可以得出磁力计数据的方向与实际方向的偏差----------------偏差值mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);//计算南北朝向向量点乘，判断同向或反向mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);//若反向，直接给最大误差if(mag_err_dot_prudoct<0){mag_yaw_err = my_sign(mag_yaw_err) *1.0f;//最大误差是1}           //}

#endif

 for(u8 i = 0;i<3;i++){

//没有定义USE_EST_DEADZONE
#ifdef USE_EST_DEADZONE //故不进来
if(state->G_reset == 0 && state->obs_en == 0)
{
vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
}
#endif
//定义了USE_LENGTH_LIM

#ifdef USE_LENGTH_LIM //限幅

     if(acc_norm_l>1060 || acc_norm_l<900)//当加速度值过大或者过小，就忽略加速度计的误差，即设为0{vec_err[X] = vec_err[Y] = vec_err[Z] = 0;}

#endif

     //误差积分vec_err_i[i] +=  LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;//加速度计的误差累计，且乘上了积分常数// 构造增量旋转（含融合纠正）。
//    d_angle[X] = (gyr[X] + (vec_err[X]  + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
//    d_angle[Y] = (gyr[Y] + (vec_err[Y]  + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
//    d_angle[Z] = (gyr[Z] + (vec_err[Z]  + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;

#ifdef USE_MAG
//用叉积误差来做PI修正陀螺仪零偏，即抵消陀螺仪读数中的偏移量
//用加速度计方向的偏差PI 和磁力计偏航方向的偏差P 来修正陀螺仪的角速度 = d_angle[i]
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ; //不进入这里
#endif
}

//*********************************** 姿态更新 *********************************************************************//基于四元数的姿态解算的互补滤波算法
imu->w = imu->w            - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
imu->x = imu->w*d_angle[X] + imu->x            + imu->y*d_angle[Z] - imu->z*d_angle[Y];
imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y            + imu->z*d_angle[X];
imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;//四元数规范化（单位化）q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
imu->w *= q_norm_l;
imu->x *= q_norm_l;
imu->y *= q_norm_l;
imu->z *= q_norm_l;//*********************************** 更新结束 ***********************************************************************

/修正开关正常情况下各个修正开关都是打开的，在开头可以看出///
#ifdef USE_MAG
if(state->M_fix_en==0)//磁力计修正开关=0
{
mkp_use = 0;//比例P常数 = 0
state->M_reset = 0;//罗盘清除复位标记
}
else//磁力计修正开关=1
{
if(state->M_reset)//罗盘修正复位
{
//通过增量进行对准
mkp_use = 10.0f;//修正复位
if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.01f)
{
state->M_reset = 0;//误差小于2的时候，清除复位标记
}
}
else //罗盘修正不复位
{
mkp_use = state->mkp; //正常修正
}
}
#endif

 if(state->G_fix_en==0)//重力方向修正开关=0{kp_use = 0;//不修正}else{if(state->G_reset == 0)//正常修正{         kp_use = state->gkp;ki_use = state->gki;}else//快速修正，通过增量进行对准{kp_use = 10.0f;ki_use = 0.0f;

// imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
// imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
// imu->est_acc_h[X] = imu->est_acc_h[Y] =0;

         //计算静态误差是否缩小

// imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
// imu_reset_val -= 0.01f;
imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));

         imu_reset_val = LIMIT(imu_reset_val,0,1.0f);if((imu_reset_val < 0.02f) && (state->M_reset == 0))//加速度计水平误差绝对值之和过小，且磁力计的修正开关=0---------相当于重力修正开关没必要打开{//计时                          reset_cnt += 2;if(reset_cnt>400)//如果计时到了一定值，效果还是很好并且磁力计修正开关关掉时，那就不用重力修正了{                                    //如果加速度计的误差小，但是磁力计的修正开关还是打开的，那重力修正开关就会继续打开reset_cnt = 0;state->G_reset = 0;//已经对准，清除复位标记}}else{reset_cnt = 0;}}}

}

static float t_temp;
//计算欧拉角
void calculate_RPY()
{
///输出姿态角///

 t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算{imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f; imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f; }

}

总结：该文件就只做了两件事：1、姿态更新 2、将姿态阵解算出姿态角
具体说明：
1、姿态更新
首先我们可以通过参考书知道姿态更新的方程式，就是与角速度有关。
对于角速度的测量主要依靠陀螺仪的数据，但它是有误差的，在这段代码中使用了加速度计和磁力计来修正陀螺仪的误差。修正完后根据更新式求出更新后的姿态阵。
2、由姿态阵解算出姿态角，可以直接根据公式求解出。

匿名飞控STM32版代码整理之Ano_Imu.c相关推荐

匿名飞控码STM32版代码整理之Ano_AttCtrl.c
代码 #include "Ano_AttCtrl.h" #include "Ano_Imu.h" #include "Drv_icm20602.h&q ...
ANO匿名飞控STM32代码解读（一）任务调度——Ano_Scheduler.c
我所学习的代码是匿名飞控使用STM32芯片ANO_PioneerPro-20190825的版本. 匿名飞控的整体代码是跑裸机的,任务调度是用STM32F4芯片中的系统时钟计时,做了一个任务调度系统,举 ...
匿名飞控TI版_姿态解算
匿名飞控TI版_姿态解算准备电赛准备大创先看看匿名姿态解算的代码文章目录匿名飞控TI版_姿态解算一,姿态解算原理 1.介绍 2.方向余弦矩阵 (1)方向余弦 (2)DCM矩阵 3.欧拉角 ...
ANO匿名飞控STM32代码解读（二）数据传输——Ano_DT.c
我使用代码对应的上位机版本为v6,具体通信协议格式在打开V6版本上位机–帮助信息–通信协议里可以看到. 这部分数据传输的核心简单的解帧.这部分的讲解以及匿名的通信协议的简介可以看另一篇文章**< ...
【开源飞控】匿名飞控TI版解析（1）
准备电赛的飞控题,买来了匿名的飞控学习一下,这里整理了一下匿名飞控中比较关键的几部分,学习了一下原理,然后代码解读都写注释里了,篇幅较长. 目录一.遥控器信号接收 1.代码解读 2.ppm原理二. ...
匿名飞控TI版_PID部分，串级PID，微分先行，前馈控制
文章目录 PID介绍有趣的故事控制模型位置式PID和增量式PID 位置式PID 增量式PID 串级PID 前馈控制微分先行匿名代码分析 PID介绍 PID介绍有趣的故事 PID的故事 \s ...
匿名飞控openmv寻色块代码分析
本人最近一段时间在学习匿名飞控(使用的是匿名拓空者),也去网上看了很多资料和一些大佬的见解,学到了一些东西,所以想和大家一起学习讨论一下,互帮互助,共同进步.同时也是为了方便查找,防止遗忘.若是我有什 ...
matlab滤波器代码,自适应滤波器设计及Matlab实现附程序代码整理版.doc
自适应滤波器设计及Matlab实现附程序代码整理版.doc 维纳自适应?滤波器设计?及Matl?ab实现摘要本文从随机?噪声的特性?出发,分析了传统?滤波和自适?应滤波基本?工作原理和?性能,以 ...
ANO匿名飞控分析（2）— 任务调度
准备电赛,简单写一下匿名飞控的分析基于TM4C主控的匿名拓空者飞控,介绍见匿名科创–匿名拓空者PRO-TI版全开源飞控使用入门-TM4C123 一.简介匿名飞控的任务调度还是比较简单的,没有操作系 ...

匿名飞控STM32版代码整理之Ano_Imu.c

匿名飞控STM32版代码整理之Ano_Imu.c相关推荐

最新文章

热门文章