经过对MK代码以及各位前辈们在论坛里留下的精华帖，最近对MK代码小有理解，望各位大神指
飞控算法：首先，陀螺仪是一个瞬时数据误差较小的传感器，在短时间内，陀螺仪的数据就能够表示当前四轴的姿态，对陀螺仪的数据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]) > 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;
} elselast_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;
} elselast_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。 还有一个疑问，就是为什么MK主函数while循环是每2ms执行一次？（只有MK代码里注释了一下，但是没说为什么？难道他们计算过每次要运行多少指令？） 2ms是时钟ticks。 mark 回复【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++;
}
}
页:
[1]