经过对MK代码以及各位前辈们在论坛里留下的精华帖，最近对MK代码小有理解，望各位大神指

hgmaizi927

飞控算法：

首先，陀螺仪是一个瞬时数据误差较小的传感器，在短时间内，陀螺仪的数据就能够表示当前四轴的姿态，对陀螺仪的数据MesswertNick进行PI控制得到电机的值MoterWert。但是陀螺仪的很大一个问题就是误差累积。因此引入加速度计，加速度计则相反，瞬时误差可能很大，数据很敏感，但是滤波后的加速度就较接近于真实值。由此加速度的积分也能够描述出四轴的姿态。怎样把一个长时间才能稳定的数据，和一个短时间误差小的数据融合在一起，真实的反映出四轴的姿态？
就是MK里的融合算法，即长期融合和实时融合。

实时融合：
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
    tmp_long /= 16;
    tmp_long2 /= 16;
   if((MaxStickNick > 15) || (MaxStickRoll > 15))
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }
   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
    {
    tmp_long  /= 3;
    tmp_long2 /= 3;
    }

#define AUSGLEICH 32
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;

    Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
长期融合：
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++     
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;

    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
（ps、LageKorrekturNick是在每个程序周期都会补偿到Mess_IntegralNick变量中，它应该是对陀螺仪漂移的估计，然后再将实际测得的陀螺仪数值减去这个漂移估计，得到一个较接近真实值的数据）
有漂移的话说明这个时候应该再对陀螺仪重新设定中立点，防止数据误差越来越大，大到无法控制了。
下面一段代码就是长期融合里的陀螺仪中立点修改：
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
        {
                /*必须连续两次的误差都很大，才能进入下面的if语句*/

        if(IntegralFehlerNick >  FEHLER_LIMIT2)
         {
           if(last_n_p)
           {
                   /*连续两次误差较大时，对陀螺仪漂移进行补偿*/
                   /*最后改变了LageKorrekturNick的值*/
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
            ausgleichNick = IntegralFehlerNick / 8;
            if(ausgleichNick > 5000) ausgleichNick = 5000;
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
           }
           else last_n_p = 1;
         } else  last_n_p = 0;
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
         {
           if(last_n_n)
            {
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
             ausgleichNick = IntegralFehlerNick / 8;
             if(ausgleichNick < -5000) ausgleichNick = -5000;
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
            }
           else last_n_n = 1;
         } else  last_n_n = 0;
        } else cnt = 0;
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
如果不对陀螺仪的中立点进行修改，那陀螺仪的数据误差会越来越大，实时的加速度计补偿根本不能够抵消陀螺仪的误差。

最后每0.5s修改一次陀螺仪中立点，每2ms得到一个较接近真实数据的MesswertNick。
再进行PI控制得到最终数据Moterwert。

hgmaizi927

还有一个疑问，就是为什么MK主函数while循环是每2ms执行一次？（只有MK代码里注释了一下，但是没说为什么？难道他们计算过每次要运行多少指令？）

modelfly

2ms是时钟ticks。

largeboss

mark

hgmaizi927

回复【2楼】modelfly
-----------------------------------------------------------------------

怎么看的。？
SIGNAL (SIG_OVERFLOW0)    // 8kHz
{
    static unsigned char cnt_1ms = 1,cnt = 0;
    unsigned char pieper_ein = 0;
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;

   if(!cnt--)//这个中断0.125ms运行一次。这个if语句又是10个0.125ms运行一次（1.25ms）
    {
     cnt = 9;
     cnt_1ms++;
     cnt_1ms %= 2;
     if(!cnt_1ms) UpdateMotor = 1;//这个if语句是每2个1.25ms执行一次。那算一下UpdateMotor应该是每2.5ms设置为1.周期应该是2.5ms啊。为什么都说是2ms？？
     CountMilliseconds++;
     }   
}