tmp_long /= 3;
tmp_long2 /= 3;
}
/*当偏航轴的操纵杆输入较大时候，则说明这时候偏航轴有一个角速度，为了消除有害加速度的影响，必须对这
两个数值再做一次衰减*/
if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
/*做一个限制，补偿值必须在一定的范围内。将补偿的范围限制在+-32*/
#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补偿了，Mess_IntegralNick2没有补偿，因为在后面还要用到
*/
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
} // end if of if(!Looping_Nick && !Looping_Roll)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*当 >ABGLEICH_ANZAHL(256)时候 说明测量了256次航向*/
/*变量ZaehlMessungen是在AD检测的函数中改变的，也就是说，下面这个if语句是每256个检测周期计算一次，
而不是控制周期，检测周期要高于控制周期*/
if(ZaehlMessungen >= ABGLEICH_ANZAHL)//关于时间积累的处理过程
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
if(!Looping_Nick && !Looping_Roll)
{
MittelIntegralNick /= ABGLEICH_ANZAHL;
MittelIntegralRoll /= ABGLEICH_ANZAHL;
/*计算加速度计积分的作用，在不运动时候，xy加速度计的积分应该是0，所以xy积
分而z不积分*/
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) /
ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) /
ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
#define MAX_I 0//(Poti2/10)
// 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;
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in
[EE_Parameter.Kanalbele×g[K_GIER]]) > 25))
{
/*这个参数在后面的程序中还要进行修正，修正后的值加入到陀螺仪的积分
中，可以认为这个参数是系统对于陀螺仪漂移的估计*/
LageKorrekturNick /= 2;
LageKorrekturRoll /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln//陀螺漂移的确定
/*对陀螺仪漂移的估计过程*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*前面MittelIntegralNick已经用过了，因此这里使用MittelIntegralNick2*/
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
/*有校正和没有校正的陀螺仪积分做差，即陀螺仪的漂移*/
/*IntegralNick2是没有校正的陀螺仪积分 IntegralNick是有校正的陀螺仪积分 这里的校正指的是使用加速度
计积分进行的校正*/
tmp_long = IntegralNick2 - IntegralNick;
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
/*将差值加入到Mess_IntegralNick2和Mess_IntegralRoll2中 这时Mess_IntegralNick2和Mess_IntegralRoll2
被使用*/
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
/*下面两个公式的作用就是让Mess_IntegralNick2=Mess_IntegralNick，
Mess_IntegralRoll2=Mess_IntegralRoll为下一个计算周期做准备*/
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWE×GS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
/*以下部分就是对LageKorrekturNick的修正和对陀螺仪常值误差的修正*/
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWE×GS_LIMIT)
{
if(IntegralFehlerNick > FEHLER_LIMIT2)
{
/*必须连续两次的误差都很大，才能进入下面的if语句*/
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;
上一页 [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] 下一页
本文关键字：飞行器 电脑-单片机-自动控制，电子学习 - 基础知识 - 电脑-单片机-自动控制
