返回信息流以下是直立控制获得角度的三个函数，摘自去年我们车的代码，简洁起见，删去了后期加的一些过坡的处理。
完整代码：
http://pan.baidu.com/s/1kVJllLP
（很多地方未写注释，因此仅供参考）
void GetGyroResult(){ //读陀螺仪输出
s32 s32_AD_result_X,s32_AD_result_Y;
s32_AD_result_Y=ad_ave(ENC_AD,ENC_Y_CH,ADBITS,50); //采样50次，均值滤波
g_f_gyro_result_Y=s32_AD_result_Y-g_f_gyro_offset_Y; //减去0偏
g_f_gyro_result_Y*=GYRO_ANGLE_RATIO; //将陀螺仪的输出归一到0-180度。比例系数的调整参考官方方案，与AD位数有关。
}
void GetGravityResult(){ //读加速度计输出
s32 s32_AD_result=GET_ADVAL(ACCER_AD,ACCER_Z_CH); //读加速度计的值
g_f_gravity_result=s32_AD_result-g_u16_gravity_offset; //减去0偏
g_f_gravity_angle=g_f_gravity_result*GRAVITY_ANGLE_RADIO; //同理，将加速度计的读数归一到0-180°。
}
void GetAngle(){ //互补滤波函数
float f_angle_sub;
g_f_angle=g_f_angle_integral;
f_angle_sub=(g_f_gravity_angle-g_f_angle)/g_u16_Tg; //求陀螺仪积分出来的角度与加速度计的角度的差值，并除以补偿系数，得到加速度计对陀螺仪的校准量。补偿系数取1-4，补偿系数越大，静态误差越大。我们最后取了Tg=1;
g_f_angle_integral+=(f_angle_sub-g_f_gyro_result_Y)/ANGLE_CALCULATE_FREQENCY; //将校准量加到陀螺仪上
}
最后一个互补滤波函数，原理其实就是此图：
最后的滤波效果：（红色为滤波完后的角度，可看出毛刺不多。蓝色为加速度计输出的角度）
可看出，红色的角度基本能跟随上加速度计的角度。
当然，这个图还有点问题（因为加速度计零偏不对的问题，导致两个波形不重合。）
（我手头只剩下这个图了，见谅）
这是一条镜像帖。来源：北邮人论坛 / smartcar / #219同步于 2017/2/26
Smartcar机器人发帖
互补滤波代码示例
Ncer
2017/2/26镜像同步0 回复
订阅后，新回复会通过你的通知中心匿名送达。
0 条回复
暂无回复 · 你可以订阅本帖等待新回复。