if(DubWiseKeys[0] & 16)
ExternHoehenValue--;
StickNick += ExternStickNick / 8;
StickRoll += ExternStickRoll / 8;
StickGier += ExternStickGier;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)//同上，具有扩展功能的控制输入
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
/*陀螺仪积分比例为零，应该是Looping的情况?*/
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)
IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?//这里是在空中转圈的情况
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
else
{
{
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Links = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
else
{
if(Looping_Rechts) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
else
{
if(Looping_Oben) // Hysterese
{
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Oben = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
else
{
if(Looping_Unten) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Unten = 0;
}
}
/*不应该出现轴都是Looping的情况*/
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0;
Looping_Rechts = 0;} else Looping_Nick = 0;
} // end if of if(!NewPpmData-- || Notlandung)
if(Looping_Roll) beeptime = 100;
if(Looping_Roll || Looping_Nick)
{
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
{
/*如果出现紧急降落，则将三个期望位置全部置零，即让飞行器向最稳定的方向调整，同时改变控制参
数，并且不让飞行器处在空中打转的状态*/
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = 0.1;
IntegralFaktor = 0.005;
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen//加速度信号的积分校准
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ABGLEICH_ANZAHL 256L
/*计算陀螺仪积分的积分，为了和加速度计的积分做比较，进行角速率的补偿和陀螺仪中立点的修正*/
MittelIntegralNick += IntegralNick;
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
/*在空中打转过程中，让所有的积分项都为零，因为机动过程会产生很大的误差，因此需要尽快结束其控制，然
后自动调平。 */
if(Looping_Nick || Looping_Roll)
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll)
{
long tmp_long, tmp_long2;
/*使用加速度计的值去补偿陀螺仪的积分，这里必须知道EE_Parameter.GyroAccFaktor参数，才能够知道补偿了
多少*/
/*其中IntegralNick应该是陀螺仪积分
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
是滤波后的加速度，用当前加速度和上次的加速度平均 */
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;
/*如果历史最大摇杆的量比较大，则说明在前段时间内飞行器的姿态可能不为0，这就导致加速度计的输出受到
有害加速度的影响，因此必须加速度计和陀螺仪积分差值的基础上做一次衰减*/
if((MaxStickNick > 15) || (MaxStickRoll > 15))
{
上一页 [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] 下一页
本文关键字：飞行器 电脑-单片机-自动控制，电子学习 - 基础知识 - 电脑-单片机-自动控制
