eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)
eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
}
/*将所有数据清零，这里带2的变量都是加入了陀螺仪漂移补偿值之后得到的角度*/
Mess_IntegralNick = 0;
Mess_IntegralNick2 = 0;
Mess_IntegralRoll = 0;
Mess_IntegralRoll2 = 0;
Mess_Integral_Gier = 0;
MesswertNick = 0;
MesswertRoll = 0;
MesswertGier = 0;
StartLuftdruck = Luftdruck;
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50; //
/*从EEPROM中读取陀螺仪积分到达90°时候的值，并储存，当得到的姿态角度大于这个范围时，说明超过了90°
，就要进行相应的处理*/
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
ExternHoehenValue = 0;
}
///////////////////////////////
//函数描述 ：求参数的平均数值
//////////////////////////////
//############################################################################
// Bearbeitet die Messwerte
void Mittelwert(void)
// 根据测量值 计算陀螺仪和加速度计数据
//############################################################################
{
static signed long tmpl,tmpl2;
/*将陀螺仪数据减去常值误差，得到实际的叫速率的倍数*/
MesswertGier = (signed int) AdNeutralGier - AdWertGier;
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
MesswertNick = (signed int) AdWertNick - AdNeutralNick;
//DebugOut.Analog[26] = MesswertNick;
DebugOut.Analog[28] = MesswertRoll;
//加速度传感器输出
/*加速度计数据滤波，ACC_AMPLIFY=12 得到的Mittelwert_AccNick是加速度计数值的12倍*/
/*AdWertAccNick为测量值*/
// Beschleuni×gssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) /
2L;//具有滤波功能的方法，用当前加速度和上次的加速度平均
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
/*计算加速度计的积分，加速度计对运动十分敏感，采用加速度计积分，可以减少瞬间的运动加速度的影响*/
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
/*偏航方向无法进行滤波，因此直接进行积分得到偏航角度*/
Mess_Integral_Gier += MesswertGier;
Mess_Integral_Gier2 += MesswertGier;
/*耦合项应该是另外两个陀螺仪对这个轴上陀螺仪的影响，当四轴在稳定姿态不为水平的时候，进行偏航运动时
候所进行的补偿*/
/*假设目前的俯仰角是30°，而横滚角是0°，这时候如保持俯仰和横滚轴没有任何运动，而将偏航轴转动90°
，那么实际的俯仰角就会变为0°，横滚角就会变为30°
但是，按照目前的算法，由于俯仰和横滚方向没有运动，因此就不会有陀螺仪的积分，俯仰和横滚角是不变的
，这就是采用陀螺仪直接积分测角度的不完善性，这时候
使用加速度计对姿态进行修正能够起到作用，但是需要一段时间，使用下面的这段话，就是将偏航轴的运动耦
合在另外两个轴上，使姿态角度能够迅速收敛到真实值上*/
/*注：使用四元数法进行姿态结算可以避免出现这种问题，但这种方法需要有准确的陀螺仪和加表的数学模型，四元数法还需要进行大量的矩阵计算，
而且对四元数姿态进行加速度计的融合不太方便*/
if(!LooPINg_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig &
CFG_ACHSENKOPPLUNG_AKTIV))//不在俯仰滚转控制循环中
{
tmpl = Mess_IntegralNick / 4096L;
tmpl *= MesswertGier;
tmpl *= Parameter_AchsKopplung1; //125
tmpl /= 2048L;
tmpl2 = Mess_IntegralRoll / 4096L;
tmpl2 *= MesswertGier;
tmpl2 *= Parameter_AchsKopplung1;
tmpl2 /= 2048L;
}
else tmpl = tmpl2 = 0;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertRoll += tmpl;
MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L;
Mess_IntegralRoll2 += MesswertRoll;
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
/*积分超过半圈的情况*/
if(Mess_IntegralRoll > Umschlag180Roll)
{
Mess_IntegralRoll = -(Umschlag180Roll - 10000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(Mess_IntegralRoll <-Umschlag180Roll)//一样
{
Mess_IntegralRoll = (Umschlag180Roll - 10000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(AdWertRoll < 15) MesswertRoll = -1000;
if(AdWertRoll < 7) MesswertRoll = -2000;
if(PlatinenVersion == 10)
{
if(AdWertRoll > 1010) MesswertRoll = +1000;
if(AdWertRoll > 1017) MesswertRoll = +2000;
}
else
{
if(AdWertRoll > 2020) MesswertRoll = +1000;
if(AdWertRoll > 2034) MesswertRoll = +2000;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick -= tmpl2;
MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
Mess_IntegralNick2 += MesswertNick;
/*LageKorrekturNick是通过加速度计积分和角速率积分的积分进行做差比较得到
的,*/
Mess_IntegralNick += MesswertNick - LageKorrekturNick;
if(Mess_IntegralNick > Umschlag180Nick)
{
Mess_IntegralNick = -(Umschlag180Nick - 10000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(Mess_IntegralNick <-Umschlag180Nick)
{
Mess_IntegralNick = (Umschlag180Nick - 10000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(AdWertNick < 15) MesswertNick = -1000;
if(AdWertNick < 7) MesswertNick = -2000;
if(PlatinenVersion == 10)
{
if(AdWertNick > 1010) MesswertNick = +1000;
if(AdWertNick > 1017) MesswertNick = +2000;
}
else
{
if(AdWertNick > 2020) MesswertNick = +1000;
上一页 [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] 下一页
本文关键字：飞行器 电脑-单片机-自动控制，电子学习 - 基础知识 - 电脑-单片机-自动控制
