作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准2（磁力计如何校正航向）

导：根据我前面的视频又说过这个流程：姿态角来自四元素，四元素取决于陀螺仪，陀螺仪存在温度漂移、零点漂移和时间积分误差，所以使用加计（理想与实际的err值）来pi校准陀螺；

现在，磁力计对航向角yaw的校准，就是同样的道理：使用磁力计（理想与实际的err）来校准陀螺；

故：现在的err构成为：现在重点分析的计算过程；

1.既然磁力计的校准与加速度计的校准具有相似性，同时，此过程就是姿态结算的又一次呈现，朋友们可以先看我的视频总结；再看此文豁然开朗，印象更加深刻；下发连接如下：

https://blog.csdn.net/xiaoxilang/article/details/80326013 博客文章结尾有连接！

2.分析err的如下图：

3.代码分析:请您细看1-9点标注之处，是代码关键点，对照我之前的视频，可相互印证！

void AHRSupdate(unsigned char ahrs_flag)
{float norm,Spin_rate,P_gain,W_yaw_P[3];float change_limit,W_P[3];float q0,q1,q2,q3;float q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;float mag_nx,mag_ny,mag_nz,mag_bx,mag_by,true_my,true_mz;float f_bx,f_by,f_bz;float err1_x,err1_y,err1_z,err2_x,err2_y,err2_z;float gx,gy,gz,ax,ay,az,mx,my,mz;
//1.9轴原始数据获得-----------------------------gx = alldata.ina_gyro1;      //rightgy = alldata.ina_gyro2;     //frontgz = alldata.ina_gyro3;     //upmx = alldata.ina_mag1; //rightmy = alldata.ina_mag2; //frontmz = alldata.ina_mag3; //up ax = alldata.ina_acc1;//x_acc_filter;//outPut_buf[0][6];   // rightay = alldata.ina_acc2;//outPut_buf[1][6];   // frontaz = alldata.ina_acc3;//outPut_buf[2][6];   // up
//2.计算p参数：gyro_drift_limit = 0.01f*rad,Spin_rate_limit1 = 10.0f*rad,Spin_rate_limit2 = 50.0f*rad      //计算P_gainSpin_rate = __sqrtf(gx*gx + gy*gy + gz*gz);if (Spin_rate < Spin_rate_limit1)P_gain = 1.0f;if (Spin_rate > Spin_rate_limit2)P_gain = 5.0f;if ((Spin_rate >= Spin_rate_limit1)&&(Spin_rate <= Spin_rate_limit2))P_gain = Spin_rate/(Spin_rate_limit1);
//3.因为陀螺需要加计个磁力计校准：所以先归一化加计和磁力计三轴数据  // normalise the measurementsnorm = __sqrtf(ax*ax + ay*ay + az*az);       ax = ax / norm;ay = ay / norm;az = az / norm;norm = __sqrtf(mx*mx + my*my + mz*mz);          mx = mx / norm;my = my / norm;mz = mz / norm;q0 = ahrs_q[0];q1 = ahrs_q[1];q2 = ahrs_q[2];q3 = ahrs_q[3];q0q1 = q0*q1;q0q2 = q0*q2;q0q3 = q0*q3;q1q1 = q1*q1;q1q2 = q1*q2;q1q3 = q1*q3;q2q2 = q2*q2;   q2q3 = q2*q3;q3q3 = q3*q3;
//4.准备数据以求磁力计的实际和理想mag_nx = 2.0f*mx*(0.5f - q2q2 - q3q3) + 2.0f*my*(q1q2 - q0q3) + 2.0f*mz*(q1q3 + q0q2);mag_ny = 2.f*mx*(q1q2 + q0q3) + 2.0f*my*(0.5f - q1q1 - q3q3) + 2.0f*mz*(q2q3 - q0q1);mag_nz = 2.0f*mx*(q1q3 - q0q2) + 2.0f*my*(q2q3 + q0q1) + 2.0f*mz*(0.5f - q1q1 - q2q2);         true_my = __sqrtf(mag_nx*mag_nx+mag_ny*mag_ny);true_mz = mag_nz;     mag_bx = 2.0f*true_my*(q1q2+q0q3)+2.0f*true_mz*(q1q3-q0q2);mag_by = 2.0f*true_my*(0.5f-q1q1-q3q3)+2.0f*true_mz*(q2q3+q0q1);f_bx = -2.0f*(q1q3-q0q2);f_by = -2.0f*(q2q3+q0q1);f_bz = -2.0f*(0.5f-q1q1-q2q2);//5.加计误差：利用叉乘计算重力加速度参考向量经过Tbn矩阵转换到载体系后和实测的载体系加速度之间的误差err1_x = ay*f_bz - az*f_by;err1_y = az*f_bx - ax*f_bz;err1_z = ax*f_by - ay*f_bx;

      err2_x = 0;//6.磁力计误差：利用差乘...注意：只校航向！！！err2_y = 0;err2_z = mx*mag_by - my*mag_bx;W_yaw_P[0] = err2_x*P_gain*Kp_yaw;//7.磁力计的pi校准W_yaw_P[1] = err2_y*P_gain*Kp_yaw;W_yaw_P[2] = err2_z*P_gain*Kp_yaw;if (Spin_rate<Spin_rate_limit1){W_sum_I[0] = W_sum_I[0]+err2_x*Ki_yaw+err1_x*Ki_acc;W_sum_I[1] = W_sum_I[1]+err2_y*Ki_yaw+err1_y*Ki_acc;W_sum_I[2] = W_sum_I[2]+err2_z*Ki_yaw+err1_z*Ki_acc;W_sum_I_time++;}if (W_sum_I_time>=50){change_limit = gyro_drift_limit*W_sum_I_time*T;W_sum_I[0] = Math_fConstrain(W_sum_I[0],-change_limit,change_limit);W_sum_I[1] = Math_fConstrain(W_sum_I[1],-change_limit,change_limit);W_sum_I[2] = Math_fConstrain(W_sum_I[2],-change_limit,change_limit);W_I[0] = W_I[0]+W_sum_I[0];W_I[1] = W_I[1]+W_sum_I[1];W_I[2] = W_I[2]+W_sum_I[2];W_sum_I[0] = 0;W_sum_I[1] = 0;W_sum_I[2] = 0;W_sum_I_time = 0;}W_P[0] = err1_x*P_gain*Kp_acc;//8.加计的pi校准W_P[1] = err1_y*P_gain*Kp_acc;W_P[2] = err1_z*P_gain*Kp_acc;gx = gx+W_I[0]+W_P[0]+W_yaw_P[0];//9.陀螺的最终校准结果gy = gy+W_I[1]+W_P[1]+W_yaw_P[1];gz = gz+W_I[2]+W_P[2]+W_yaw_P[2];   ahrs_qLonge_Kuta(gx, gy, gz);//10.由陀螺计算四元素norm_ahrsq();  //11.四元素归一化,为下一次过程准备数据！！！  //12.由四元素到姿态角  ，请见视频说明！！！！
}

4.参考技术文档

陀螺仪、加速度计、磁力计的介绍
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml
磁力计工作原理介绍

http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html

四旋翼姿态解算——互补滤波法补充（融合磁力计）

https://blog.csdn.net/hongbin_xu/article/details/59110226

作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准2（磁力计如何校正航向）相关推荐

作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准1（磁力计如何校正航向）
文档参考:上海丙寅电子有限公司技术文档导语:磁力计传感器取自于大地磁感应的强度hx,hy,hz: 1如何得到罗盘的方位角:怎么才能够从简单的 3 轴数据得到罗盘的方位角 1)当 3 轴磁力计工作时 ...
cfnet用于嵌入式_做嵌入式驱动的，你一定要挺住!
搞嵌入式驱动开发一年多了,项目做的不少: 从网络驱动到CAN驱动.从Dataflash驱动到NAND/NOR FLASH驱动.从CF/IDE驱动到各种嵌入式文件系统.从LCD/VGA驱动到音频驱动.从 ...
做嵌入式的必须学Android吗
做嵌入式的必须学Android吗 Android方向适合哪些人呢?适合那些已经在自己领域有了一定的工作经验的人,适合作为自己的拓展,适合提升自己的能力,譬如说已经做三年Linux驱动,就可以尝试拓展去 ...
做嵌入式编程，为什么用的是C语言而不是C++呢？
1.前言几十年来,嵌入式软件工程师们一直在争论是否应该使用C++替代C语言进行开发工作.根据2019年IEEE和github的市场调查显示,微控制器制造商提供的软件大部分都是由C语言编写. 事实上, ...
python适合做嵌入式开发吗_python可以做嵌入式吗
Python是一种计算机程序设计语言.是一种面向对象的动态类型语言,最初被设计用于编写自动化脚本(shell),随着版本的不断更新和语言新功能的添加,越来越多被用于独立的.大型项目的开发. 近些年来P ...
做嵌入式，如果爱就全心投入，否则就放弃！
不少人或许还并不是很清晰地知道什么是嵌入式,IEEE对它定义的原文是:DevicesUsedtoControl,MonitororAssisttheOperationofEquipment,Machi ...
做嵌入式硬件工程师最重要的是什么
做嵌入式硬件工程师最重要的是什么呢? 做嵌入式工程师不仅需要扎实的理论基础还得需要丰厚的经验,具备那种大型的.高频CPU.多层PCB板的设计经验.要熟悉模拟电路设计.数字电路设计.电磁波理论等.熟悉常 ...
MicroPython做嵌入式开发的优缺点
关注+星标公众号,不错过精彩内容编排 | strongerHuang 微信公众号 | 嵌入式专栏 Python的火热程度相信大家都看到了,自然,MCU和嵌入式也紧跟"潮流",随之 ...
做嵌入式linux驱动的前途何在
做嵌入式linux驱动的前途何在? 或者说,怎么才能做好这一行? 我做嵌入式驱动开发已经一年多了,感觉没学到多少东西. 从网络驱动到CAN驱动.从dataflash驱动到NAND/NOR FLASH驱 ...
如何做嵌入式人工智能
如何做嵌入式人工智能人工智能技术与咨询来源:PerfXLab澎峰科技澎峰科技是中科院的技术团队,做过国际知名的开源矩阵计算库OpenBLAS,获得过中国计算机学会科技进步二等奖.中国科学院杰出科 ...

作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准2（磁力计如何校正航向）

作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准2（磁力计如何校正航向）相关推荐

最新文章

热门文章