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