{ // Starten
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(++delay_einschalten > 200) {
delay_einschalten = 200; modell_fliegt = 1; MotorenEin = 1; sollGier = 0;
Mess_Integral_Gier = 0; Mess_Integral_Gier2 = 0; Mess_IntegralNick = 0; Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick; Mess_IntegralRoll2 = IntegralRoll; SummeNick = 0; SummeRoll = 0; } } else
delay_einschalten = 0;//没事，就让其延迟关闭为0 //Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Auschalten /*切换*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) {
if(++delay_ausschalten > 200) // nicht sofort {
MotorenEin = 0;
delay_ausschalten = 200; modell_fliegt = 0; } }
else delay_ausschalten = 0;
}
} // end if of else if(SenderOkay > 140)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!NewPpmData-- || Notlandung) {
int tmp_int;
static int stick_nick,stick_roll;//俯仰杆，倾斜杆
ParameterZuordnung();
/*新老数据滤波混合，这里改变的应该是期望角位置，必须知道EE_Parameter.Stick_P的数值才可以得到滤波效 果*/
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] *
EE_Parameter.Stick_P) / 4; //新数据和老数据混合起滤波作用
/*将期望角位置的微分加入操纵杆变量上，这里必须知道EE_Parameter.Kanalbelegung[K_ROLL]的求法，和
EE_Parameter.Stick_D得数值*/
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;//增加上微分
量，用于提高反应的快速性。
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] *
EE_Parameter.Stick_P) / 4;
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
/*用此记录历史上的最大给杆量，如果给杆量很小，则Max数值会不断减小，用于在后面给陀螺仪积分做补偿时
，对加速度计数据和陀螺仪积分的差值做衰减*/
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; /*如果在降落过程中，则数据为0，也就是说降落的过程中不需要衰减，降落时候的保持位置全部为0，所以不需
要衰减*/ if(Notlandung) {
MaxStickNick = 0; MaxStickRoll = 0; }
/*可以认为是控制参数，前一个是陀螺仪的比例项（速率环参数） 后一个是陀螺仪积分即姿态角的比例（位置
环参数）*/
GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / 256.0; IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Digitale Steuerung per DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8)//为了增加杆的输入的丰富性，提供了扩展
的杆的描述，对最终杆的描述更加丰富。 if(DubWiseKeys[1]) beeptime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0;
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
else tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
if(DubWiseKeys[0] & 8) ExternStickGier = 50; else if(DubWiseKeys[0] & 4) ExternStickGier =-50; else
ExternStickGier = 0;
if(DubWiseKeys[0] & 2) ExternHoehenValue++;
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.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; else { {
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Links = 0; } }
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; else {
if(Looping_Rechts) // Hysterese {
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Rechts = 0; } }
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; else {
if(Looping_Oben) // Hysterese {
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Oben = 0; } }
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;