MK四轴飞控里面的若干问题，请教！谢谢

小笨蛋

想做四轴，硬件已经搭好，现在研究飞控了，MK的飞控好复杂，看了好多遍还有好多地方不懂，前来请教一下。

1.         // Coupling fraction
   
2.         if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
   
3.         {
   
4.                 tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L;
   
5.                 tmpl *= FCParam.Yaw_PosFeedback;
   
6.                 tmpl /= 4096L;
   
7.                 tmpl2 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L;
   
8.                 tmpl2 *= FCParam.Yaw_PosFeedback;
   
9.                 tmpl2 /= 4096L;
   
10.                 if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
    
11.         }
    
12.         else  tmpl = tmpl2 = 0;

复制代码

这个地方怎么把Reading_GyroYaw 和Reading_IntegralGyroNick两个轴乘到一起？这有什么意义？

1.                 int32_t tmp_long, tmp_long2;
   
2.                 if(FCParam.Kalman_K != -1)
   
3.                 {
   
4.                         // determine the deviation of gyro integral from averaged acceleration sensor
   
5.                         tmp_long   = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
   
6.                         tmp_long   = (tmp_long * FCParam.Kalman_K) / (32 * 16);
   
7.                         tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
   
8.                         tmp_long2  = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
   
9. 
   
10.                         if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
    
11.                         {
    
12.                                 tmp_long  /= 2;
    
13.                                 tmp_long2 /= 2;
    
14.                         }
    
15.                         if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
    
16.                         {
    
17.                                 tmp_long  /= 3;
    
18.                                 tmp_long2 /= 3;
    
19.                         }
    
20.                         // limit correction effect
    
21.                         if(tmp_long >  (int32_t)FCParam.Kalman_MaxFusion)  tmp_long  = (int32_t)FCParam.Kalman_MaxFusion;
    
22.                         if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion)  tmp_long  =-(int32_t)FCParam.Kalman_MaxFusion;
    
23.                         if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
    
24.                         if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
    
25.                 }

复制代码

这个地方 (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);陀螺仪和加速度计数据可以直接比较吗？难道加速度计不需要用反正切得出角度？

1.                 if(!Looping_Nick && !Looping_Roll && !FunnelCourse)
   
2.                 {
   
3.                         // Calculate mean value of the gyro integrals
   
4.                         MeanIntegralNick /= BALANCE_NUMBER;
   
5.                         MeanIntegralRoll  /= BALANCE_NUMBER;
   
6. 
   
7.                         // Calculate mean of the acceleration values
   
8.                         IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
   
9.                         IntegralAccRoll  = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
   
10. 
    
11.                         // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
    
12.                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
    
13.                         IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
    
14.                         CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
    
15.                         AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
    
16.                         // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
    
17.                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
    
18.                         IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
    
19.                         CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
    
20.                         AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
    
21. 
    
22.                         if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
    
23.                         {
    
24.                                 AttitudeCorrectionNick /= 2;
    
25.                                 AttitudeCorrectionRoll /= 2;
    
26.                         }
    

复制代码

上面不是已经                MeanIntegralNick /= BALANCE_NUMBER;                了吗，为什么还要再除一次？        AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;

1. // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   
2. // Nick-Axis
   
3. // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   
4.     DiffNick = Reading_GyroNick - StickNick;        // get difference
   
5.     if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
   
6.     else SumNick += DiffNick; // I-part for head holding
   
7.     if(SumNick >  (STICK_GAIN * 16000L)) SumNick =  (STICK_GAIN * 16000L);
   
8.     if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L);
   
9.     pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
   
10. 
    
11.     tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
    
12.     if(pd_result >  tmp_int) pd_result =  tmp_int;
    
13.     if(pd_result < -tmp_int) pd_result = -tmp_int;
    
14. 
    
15.         // Motor Front
    
16.     MotorValue = GasMixFraction + pd_result + YawMixFraction;          // Mixer
    
17.     MotorValue /= STICK_GAIN;
    
18.         if ((MotorValue < 0)) MotorValue = 0;
    
19.         else if(MotorValue > ParamSet.Gas_Max)            MotorValue = ParamSet.Gas_Max;
    
20.         if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
    
21.         Motor_Front = MotorValue;
    
22. 
    
23. // Motor Rear
    
24.         MotorValue = GasMixFraction - pd_result + YawMixFraction;     // Mixer
    
25.         MotorValue /= STICK_GAIN;
    
26.         if ((MotorValue < 0)) MotorValue = 0;
    
27.         else if(MotorValue > ParamSet.Gas_Max)            MotorValue = ParamSet.Gas_Max;
    
28.         if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
    
29.         Motor_Rear = MotorValue;

复制代码

最后这个
  DiffNick = Reading_GyroNick - StickNick;        // get difference
    if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
    else SumNick += DiffNick; // I-part for head holding
    pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
地方不懂。。
Reading_GyroNick应该是角速度，为什么能和StickNick相减呢？
后面IntegralNick 已经是积分了的，为什么 SumNick +=还要积分一次？

最后的电机控制量应该是：角速度*a+角度*b 吧？
还是    角速度*a+角度*b+∫角度*c  ？？

恳请高手指教一下，谢谢了

小笨蛋

直流电机，掌上四轴，PWM直接控制