PID定高：
高度差：offset= Althold-EstAlt
由高度环得到速度：SetVel=EXT_P*error(offset)
vel_acc=accz_tmp*accscale*accsumT (v=at)
vel=vel+vel_acc
bare_vel=(now_alt-old_Alt)/update_Frequency
得到当前速度：
vel_tmp=vel*0.9+bare_vel*0.1
得到速度差：
ErrorP=SetVel-vel_tmp
Result=P*ErrorP
ErrorI+=I*error
Result+=errorI
Result-=Int_D*(Z_tmp+Z_old)/512
|
| |