这个模块的函数是:void AltitudeCombineThread(void)。函数有点乱,部分没有看懂,加上了注释、以后再详细搞清楚吧!
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84//timeStamp in us. Thread should be executed every 2~20ms //MS5611_Altitude , should be in m. (can be fixed to abs, not relative). positive above ground //accFilted ,should be filted //高度融合模块 void AltitudeCombineThread(void) { static uint32_t tPre=0; uint32_t t; float dt; //时间差值 /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; //加速计偏移矫正 uint8_t i,j; t=micros(); //当前的计时 dt = (tPre>0)?((t-tPre)/1000000.0f):0; //时间间隔 tPre=t; if(!paOffsetInited) //wait baro to init its offset return; if(!imu.ready) return; //store err when sensor update if(Baro_ALT_Updated) //后面应该在sensor数值后加一个timeStamp,判断是否更新 { #ifndef FBM320 corr_baro = 0 - MS5611_Altitude - z_est[0]; // MS5611_Altitude baro alt, is postive above offset level. not in NED. z_est is in NED frame. #else corr_baro = relPressData*0.1 - z_est[0]; //z方向上的气压高度差 #endif Baro_ALT_Updated=0; } if(accUpdated) { imu.accb[0] -= acc_bias[0]; //获得角加速度真实值 imu.accb[1] -= acc_bias[1]; imu.accb[2] -= acc_bias[2]; for(i=0; i<3; i++) { accel_NED[i]=0.0f; //地理系角加速度 for(j=0; j<3; j++) { accel_NED[i]+=imu.DCMgb[j][i]* imu.accb[j]; //将机体系角加速度转换到地理系 } } accel_NED[2]=-accel_NED[2]; //地理系z方向的加速度 corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; //实际测量的加速度与期望的加速度差值,z方向有重力影响 accUpdated=0; } //correct accelerometer bias every time step accel_bias_corr[2] -= corr_baro * w_z_baro * w_z_baro; //通过位置来校准z方向加速度偏差 为何如此计算没看懂 //transform error vector from NED frame to body frame for (i = 0; i < 3; i++) { float c = 0.0f; for (j = 0; j < 3; j++) { c += imu.DCMgb[i][j] * accel_bias_corr[j]; //将地理系校准的加速度偏差转换到机体系 } acc_bias[i] += c * w_acc_bias * dt; //accumulate bias //将加速度偏差做Pid运算求得期望与实际的加速度偏差值 } acc_bias[2]=-acc_bias[2]; //坐标系方向朝下 /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); //物理定律距离与加速度方程算出期望的位置、速度、加速度 /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, w_z_baro); //0.5f //通过高度位置来校准 inertial_filter_correct(corr_acc[2], dt, z_est, 2, w_z_acc); //20.0f //想通过加速度偏差来校准? nav.z=z_est[0]; //z方向上期望的位置 nav.vz=z_est[1]; //z方向上期望的速度 nav.az=z_est[2]; //z方向上期望的加速度 }
最后
以上就是淡然小伙最近收集整理的关于无人机项目跟踪记录五十一----高度融合模块详解的全部内容,更多相关无人机项目跟踪记录五十一----高度融合模块详解内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供,作为学习参考使用,或来自网络收集整理,版权属于原作者所有。
发表评论 取消回复