匿名四轴程序有一块不懂！

5474208833

void height_speed_ctrl(float T,float thr,float exp_z_speed,float h_speed)
{
        static float thr_lpf;
        float height_thr;
        static float hc_acc_i,wz_speed_0,wz_speed_1;
       
        height_thr = LIMIT( 2 * thr , 0, 600 );
       
        thr_lpf += ( 1 / ( 1 + 1 / ( 0.5f *3.14f *T ) ) ) *( height_thr - thr_lpf );
       
        wz_acc_mms2 = (wz_acc/4096.0f) *10000 + hc_acc_i;//9800 *T;
       
       
       
        wz_speed_0 += my_deathzoom( (wz_acc_mms2 ) ,100) *T;
       
        hc_acc_i += 0.4f *T *( (wz_speed - wz_speed_old)/T - wz_acc_mms2 );
        hc_acc_i = LIMIT( hc_acc_i, -500, 500 );       
       
        wz_speed_0 += ( 1 / ( 1 + 1 / ( 0.1f *3.14f *T ) ) ) *( h_speed - wz_speed_0  ) ;
       
        wz_speed_1 = wz_speed_0;
       
        if( ABS( wz_speed_1 ) < 50 )
        {
                wz_speed_1 = 0;
        }
       
        wz_speed_old = wz_speed;
       
        wz_speed = wz_speed_1;
       
        wz_speed_pid_v.err = wz_speed_pid.kp *( exp_z_speed - wz_speed );                    //wz_speed_pid_v.err比例调节结果
        wz_speed_pid_v.err_d = 0.002f/T *10*wz_speed_pid.kd * (-wz_acc_mms2) *T;//(wz_speed_pid_v.err - wz_speed_pid_v.err_old);
       
        //wz_speed_pid_v.err_i += wz_speed_pid.ki *wz_speed_pid_v.err *T;
        wz_speed_pid_v.err_i += wz_speed_pid.ki *wz_speed_pid.kp *( exp_z_speed - h_speed ) *T;
        wz_speed_pid_v.err_i = LIMIT(wz_speed_pid_v.err_i,-Thr_Weight *300,Thr_Weight *300);
       
        wz_speed_pid_v.pid_out = thr_lpf + Thr_Weight *LIMIT((wz_speed_pid.kp *exp_z_speed + wz_speed_pid_v.err + wz_speed_pid_v.err_d + wz_speed_pid_v.err_i),-300,300);

        wz_speed_pid_v.err_old = wz_speed_pid_v.err;
}

正点原子

帮顶

0055

帮顶、。。。。。