德国MK开源代码的理解文档格式.docx
- 文档编号:16519489
- 上传时间:2022-11-24
- 格式:DOCX
- 页数:45
- 大小:33.43KB
德国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;
//重新开始模拟量的采集
//++++++++++++++
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 德国 MK 源代码 理解
![提示](https://static.bdocx.com/images/bang_tan.gif)