德国MK开源代码的理解文档格式.docx
《德国MK开源代码的理解文档格式.docx》由会员分享，可在线阅读，更多相关《德国MK开源代码的理解文档格式.docx（45页珍藏版）》请在冰豆网上搜索。
德国人的控制算法的核心是对角速度做PI计算，P的作用是使四轴能够产生对于外界干扰的抵抗力矩，I的作用是让四轴产生一个与角度成正比的抵抗力。
如果只有P的作用，将四轴拿在手上就会发现，四轴能够抵抗外界的干扰力矩的作用，而且这个抵抗力非常快速，只要手妄图改变四轴的转速，四轴就会产生一个抵抗力矩，但是，如果用手将四轴扳过一个角度，则四轴无法自己回到水平的角度位置，这就需要I的调节作用。
对角速度做I（积分）预算实际得到的就是角度，德国人四轴里面用的也是角度值，如果四轴有一个倾斜角度，那么四轴就会自己进行调整，直到四轴的倾角为零，它所产生的抵抗力是与角度成正比的，但是，如果只有I的作用，会使四轴迅速产生振荡，因此，必须将P和I结合起来一起使用，这时候基本上就会得到德国四轴的效果了。
在对角速度进行了PI调节之后，德国人将操纵杆的值融合到结果中去，并对得到的新的值有进行了一次PI计算，这个积分参数很小，使用这个积分的作用因为，四轴在有一个非常小的倾角的情况下产生的抵抗力矩很小，无法使四轴回到水平位置，这就会导致无论怎么手动调节微调，四轴都很难做到悬停，会不停得做水平漂移运动，这就必须不停的进行调整。
下面是我给德国四轴中飞控程序的一些注释：
void
Piep（unsigned
char
Anzahl）
{
while（Anzahl--）
if（MotorenEin）
return;
//auf
keinen
Fall
im
Flug!
beeptime
=
100;
Delay_ms（250）;
}
//函数:
SetNeutral设定传感器发出参数的中立数值，因为有漂移所以要使其每次工作都要测量出来。
//############################################################################
//
Nullwerte
ermitteln
/*设置中立点*/
SetNeutral（void）
/*加速度计中立点*/
NeutralAccX
0;
NeutralAccY
NeutralAccZ
/*陀螺仪中立点*/
AdNeutralNick
AdNeutralRoll
AdNeutralGier
Parameter_AchsKopplung1
Parameter_AchsGegenKopplung1
。
/*这个地方我还没有弄得太明白，检测中立点的函数被调用了两次，但是第一次的数据好像没有保存，只用到了
第二次的数据*/
/*记录中立点*/
CalibrierMittelwert（）;
Delay_ms_Mess（100）;
/*既然只使用了后一次的数据，为什么要进行两次记录中立点的函数*/
if（（EE_Parameter.GlobalConfig
&
CFG_HOEHENREGELUNG））
{
if（（MessLuftdruck
>
950）
||
（MessLuftdruck
<
750））
SucheLuftruckOffset（）;
//如果气压表输出在
950外750内，则设定气压初始的偏差。
}
/*将量测值作为陀螺仪的中立点*/
AdNeutralNick=
AdWertNick;
AdNeutralRoll=
AdWertRoll;
AdNeutralGier=
AdWertGier;
/*这两个参数在飞控程序中没有用到*/
StartNeutralRoll
AdNeutralRoll;
StartNeutralNick
AdNeutralNick;
if（eeprom_read_byte（&
EEPromArray[EEPROM_ADR_ACC_NICK]）
4）
/*由于在函数CalibrierMittelwert（）中加速度计的输出乘以了ACC_AMPLIFY，所以这里必须处以ACC_AMPLIFY，
在这段程序中，所有的对加速度计和陀螺仪的数值的衰减或者放大都是为了让
陀螺仪积分和加速度计数值在同样的角度偏差的情况下能基本匹配，如果不匹配，那么就谈不上用加速度计来补偿陀螺仪积分了*/
abs（Mittelwert_AccRoll）
/
ACC_AMPLIFY;
abs（Mittelwert_AccNick）
Aktuell_az;
else
/*如果发现变化不大，则仍然储存上一次的*/
（int）eeprom_read_byte（&
*
256
+
（int）
eeprom_read_byte（&
EEPromArray[EEPROM_ADR_ACC_NICK+1]）;
EEPromArray[EEPROM_ADR_ACC_ROLL]）
EEPromArray[EEPROM_ADR_ACC_ROLL+1]）;
EEPromArray[EEPROM_ADR_ACC_Z]）
EEPromArray[EEPROM_ADR_ACC_Z+1]）;
/*将所有数据清零，这里带2的变量都是加入了陀螺仪漂移补偿值之后得到的角度*/
Mess_IntegralNick
Mess_IntegralNick2
Mess_IntegralRoll
Mess_IntegralRoll2
Mess_Integral_Gier
MesswertNick
MesswertRoll
MesswertGier
StartLuftdruck
Luftdruck;
HoeheD
Mess_Integral_Hoch
KompassStartwert
KompassValue;
GPS_Neutral（）;
50;
/*从EEPROM中读取陀螺仪积分到达90°
时候的值，并储存，当得到的姿态角度大于这个范围时，说明超过了90°
，就要进行相应的处理*/
Umschlag180Nick
（long）
EE_Parameter.WinkelUmschlagNick
2500L;
Umschlag180Roll
EE_Parameter.WinkelUmschlagRoll
ExternHoehenValue
///////////////////////////////
//函数描述
：
求参数的平均数值
//////////////////////////////
Bearbeitet
die
Messwerte
Mittelwert（void）
根据测量值
计算陀螺仪和加速度计数据
static
signed
long
tmpl,tmpl2;
/*将陀螺仪数据减去常值误差，得到实际的叫速率的倍数*/
（signed
int）
-
AdWertRoll
AdWertNick
//DebugOut.Analog[26]
MesswertNick;
DebugOut.Analog[28]
MesswertRoll;
//加速度传感器输出
/*加速度计数据滤波，ACC_AMPLIFY=12
得到的Mittelwert_AccNick是加速度计数值的12倍*/
/*AdWertAccNick为测量值*/
Beschleunigungssensor
++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick
（（long）Mittelwert_AccNick
1
（（ACC_AMPLIFY
（long）AdWertAccNick）））
2L;
//具有滤波功能的方法，用当前加速度和上次的加速度平均
Mittelwert_AccRoll
（（long）Mittelwert_AccRoll
（long）AdWertAccRoll）））
Mittelwert_AccHoch
（（long）Mittelwert_AccHoch
（（long）AdWertAccHoch））
/*计算加速度计的积分，加速度计对运动十分敏感，采用加速度计积分，可以减少瞬间的运动加速度的影响*/
IntegralAccNick
+=
ACC_AMPLIFY
AdWertAccNick;
IntegralAccRoll
AdWertAccRoll;
IntegralAccZ
Aktuell_az
NeutralAccZ;
Gier
++++++++++++++++++++++++++++++++++++++++++++++++
/*偏航方向无法进行滤波，因此直接进行积分得到偏航角度*/
MesswertGier;
Mess_Integral_Gier2
/*耦合项应该是另外两个陀螺仪对这个轴上陀螺仪的影响，当四轴在稳定姿态不为水平的时候，进行偏航运动时候所进行的补偿*/
/*假设目前的俯仰角是30°
，而横滚角是0°
，这时候如保持俯仰和横滚轴没有任何运动，而将偏航轴转动90°
，那么实际的俯仰角就会变为0°
，横滚角就会变为30°
，
但是，按照目前的算法，由于俯仰和横滚方向没有运动，因此就不会有陀螺仪的积分，俯仰和横滚角是不变的，这就是采用陀螺仪直接积分测角度的不完善性，这时候使用加速度计对姿态进行修正能够起到作用，但是需要一段时间，使用下面的这段话，就是将偏航轴的运动耦合在另外两个轴上，使姿态角度能够迅速收敛到真实值上*/
/*注：
使用四元数法进行姿态结算可以避免出现这种问题，但这种方法需要有准确的陀螺仪和加表的数学模型，四元数法还需要进行大量的矩阵计算，而且对四元数姿态进行加速度计的融合不太方便*/
if（!
Looping_Nick
!
Looping_Roll
（EE_Parameter.GlobalConfig
CFG_ACHSENKOPPLUNG_AKTIV））//不在俯仰滚转控制循环中
tmpl
4096L;
*=
Parameter_AchsKopplung1;
//125
/=
2048L;
tmpl2
else
Roll
tmpl;
（tmpl2*Parameter_AchsGegenKopplung1）/512L;
LageKorrekturRoll;
/*积分超过半圈的情况*/
if（Mess_IntegralRoll
Umschlag180Roll）
-（Umschlag180Roll
10000L）;
Mess_IntegralRoll;
-Umschlag180Roll）//一样
（Umschlag180Roll
if（AdWertRoll
15）
-1000;
7）
-2000;
if（PlatinenVersion
==
10）
1010）
+1000;
1017）
+2000;
}
2020）
2034）
Nick
-=
tmpl2;
（tmpl*Parameter_AchsGegenKopplung1）/512L;
/*LageKorrekturNick是通过加速度计积分和角速率积分的积分进行做差比较得到的,*/
LageKorrekturNick;
if（Mess_IntegralNick
Umschlag180Nick）
-（Umschlag180Nick
Mess_IntegralNick;
-Umschlag180Nick）
（Umschlag180Nick
if（AdWertNick
10）
//++++++++++++++++++++++++++++++++++++++++++++++++
ADC
einschalten
ANALOG_ON;
//重新开始模拟量的采集
//++++++++++++++
升级会员 