Rev 688 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 688 | Rev 690 | ||
---|---|---|---|
Line 69... | Line 69... | ||
69 | volatile unsigned int I2CTimeout = 100; |
69 | volatile unsigned int I2CTimeout = 100; |
70 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
70 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
71 | volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
71 | volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
72 | volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
72 | volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
73 | volatile float NeutralAccZ = 0; |
73 | volatile float NeutralAccZ = 0; |
74 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
- | |
75 | volatile long IntegralNick = 0,IntegralNick2 = 0; |
74 | volatile long IntegralNick = 0,IntegralNick2 = 0; |
76 | volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
75 | volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
77 | volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
76 | volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
78 | volatile long Integral_Gier = 0; |
77 | volatile long Integral_Gier = 0; |
79 | volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
78 | volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
Line 110... | Line 109... | ||
110 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
109 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
111 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
110 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
112 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
111 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
113 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
112 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
114 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
113 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
115 | unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
114 | unsigned char Parameter_I_Factor = 10; // Wert : 1-20 |
116 | unsigned char Parameter_UserParam1 = 0; |
115 | unsigned char Parameter_UserParam1 = 0; |
117 | unsigned char Parameter_UserParam2 = 0; |
116 | unsigned char Parameter_UserParam2 = 0; |
118 | unsigned char Parameter_UserParam3 = 0; |
117 | unsigned char Parameter_UserParam3 = 0; |
119 | unsigned char Parameter_UserParam4 = 0; |
118 | unsigned char Parameter_UserParam4 = 0; |
120 | unsigned char Parameter_UserParam5 = 0; |
119 | unsigned char Parameter_UserParam5 = 0; |
Line 154... | Line 153... | ||
154 | Parameter_AchsKopplung1 = 0; |
153 | Parameter_AchsKopplung1 = 0; |
155 | Parameter_AchsGegenKopplung1 = 0; |
154 | Parameter_AchsGegenKopplung1 = 0; |
156 | CalibrierMittelwert(); |
155 | CalibrierMittelwert(); |
157 | Delay_ms_Mess(100); |
156 | Delay_ms_Mess(100); |
158 | CalibrierMittelwert(); |
157 | CalibrierMittelwert(); |
159 | if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
158 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
160 | { |
159 | { |
161 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
160 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
162 | } |
161 | } |
Line 163... | Line 162... | ||
163 | 162 | ||
Line 220... | Line 219... | ||
220 | IntegralAccZ += Current_AccZ - NeutralAccZ; |
219 | IntegralAccZ += Current_AccZ - NeutralAccZ; |
221 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
220 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
222 | Mess_Integral_Gier += MesswertGier; |
221 | Mess_Integral_Gier += MesswertGier; |
223 | Mess_Integral_Gier2 += MesswertGier; |
222 | Mess_Integral_Gier2 += MesswertGier; |
224 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
223 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
225 | if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
224 | if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
226 | { |
225 | { |
227 | tmpl = Mess_IntegralNick / 4096L; |
226 | tmpl = Mess_IntegralNick / 4096L; |
228 | tmpl *= MesswertGier; |
227 | tmpl *= MesswertGier; |
229 | tmpl *= Parameter_AchsKopplung1; //125 |
228 | tmpl *= Parameter_AchsKopplung1; //125 |
230 | tmpl /= 2048L; |
229 | tmpl /= 2048L; |
Line 297... | Line 296... | ||
297 | IntegralNick = Mess_IntegralNick; |
296 | IntegralNick = Mess_IntegralNick; |
298 | IntegralRoll = Mess_IntegralRoll; |
297 | IntegralRoll = Mess_IntegralRoll; |
299 | IntegralNick2 = Mess_IntegralNick2; |
298 | IntegralNick2 = Mess_IntegralNick2; |
300 | IntegralRoll2 = Mess_IntegralRoll2; |
299 | IntegralRoll2 = Mess_IntegralRoll2; |
Line 301... | Line 300... | ||
301 | 300 | ||
302 | if(ParamSet.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
301 | if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Nick && !Looping_Roll) |
303 | { |
302 | { |
304 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
303 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
305 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
304 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
306 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
305 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
307 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
306 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
308 | } |
307 | } |
309 | if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
308 | if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
310 | if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
309 | if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
311 | if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
310 | if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
312 | if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
311 | if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
313 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
312 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
314 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
313 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
315 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
314 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
316 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
315 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
Line 329... | Line 328... | ||
329 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdValueAccNick; |
328 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdValueAccNick; |
330 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll; |
329 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll; |
331 | Mittelwert_AccHoch = (long)AdValueAccTop; |
330 | Mittelwert_AccHoch = (long)AdValueAccTop; |
332 | // ADC einschalten |
331 | // ADC einschalten |
333 | ADC_Enable(); |
332 | ADC_Enable(); |
334 | if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
333 | if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
335 | if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
334 | if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
336 | if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
335 | if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
337 | if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
336 | if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
338 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
337 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
339 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
338 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
340 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
339 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
341 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
340 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
Line 386... | Line 385... | ||
386 | CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100); |
385 | CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100); |
387 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255); |
386 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255); |
388 | CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,0,255); |
387 | CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,0,255); |
389 | CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255); |
388 | CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255); |
390 | CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255); |
389 | CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255); |
391 | CHK_POTI(Parameter_I_Faktor,ParamSet.I_Faktor,0,255); |
390 | CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255); |
392 | CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255); |
391 | CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255); |
393 | CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255); |
392 | CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255); |
394 | CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255); |
393 | CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255); |
395 | CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255); |
394 | CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255); |
396 | CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255); |
395 | CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255); |
Line 401... | Line 400... | ||
401 | CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
400 | CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
402 | CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
401 | CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
403 | CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
402 | CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
404 | CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255); |
403 | CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255); |
Line 405... | Line 404... | ||
405 | 404 | ||
406 | Ki = (float) Parameter_I_Faktor * 0.0001; |
405 | Ki = (float) Parameter_I_Factor * 0.0001; |
407 | MAX_GAS = ParamSet.Gas_Max; |
406 | MAX_GAS = ParamSet.Gas_Max; |
408 | MIN_GAS = ParamSet.Gas_Min; |
407 | MIN_GAS = ParamSet.Gas_Min; |
Line 457... | Line 456... | ||
457 | Notlandung = 0; |
456 | Notlandung = 0; |
458 | } |
457 | } |
459 | ROT_ON; |
458 | ROT_ON; |
460 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
459 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
461 | { |
460 | { |
462 | GasMischanteil = ParamSet.NotGas; |
461 | GasMischanteil = ParamSet.EmergencyGas; |
463 | Notlandung = 1; |
462 | Notlandung = 1; |
464 | PPM_in[ParamSet.Kanalbelegung[K_NICK]] = 0; |
463 | PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
465 | PPM_in[ParamSet.Kanalbelegung[K_ROLL]] = 0; |
464 | PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
466 | PPM_in[ParamSet.Kanalbelegung[K_GIER]] = 0; |
465 | PPM_in[ParamSet.ChannelAssignment[CH_GIER]] = 0; |
467 | } |
466 | } |
468 | else MotorenEin = 0; |
467 | else MotorenEin = 0; |
469 | } |
468 | } |
470 | else |
469 | else |
471 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
470 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // Emfang gut |
471 | // Emfang gut |
473 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
474 | if(SenderOkay > 140) |
473 | if(SenderOkay > 140) |
475 | { |
474 | { |
476 | Notlandung = 0; |
475 | Notlandung = 0; |
477 | RcLostTimer = ParamSet.NotGasZeit * 50; |
476 | RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
478 | if(GasMischanteil > 40) |
477 | if(GasMischanteil > 40) |
479 | { |
478 | { |
480 | if(modell_fliegt < 0xffff) modell_fliegt++; |
479 | if(modell_fliegt < 0xffff) modell_fliegt++; |
481 | } |
480 | } |
482 | if((modell_fliegt < 200) || (GasMischanteil < 40)) |
481 | if((modell_fliegt < 200) || (GasMischanteil < 40)) |
Line 484... | Line 483... | ||
484 | SummeNick = 0; |
483 | SummeNick = 0; |
485 | SummeRoll = 0; |
484 | SummeRoll = 0; |
486 | Mess_Integral_Gier = 0; |
485 | Mess_Integral_Gier = 0; |
487 | Mess_Integral_Gier2 = 0; |
486 | Mess_Integral_Gier2 = 0; |
488 | } |
487 | } |
489 | if((PPM_in[ParamSet.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
488 | if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorenEin == 0) |
490 | { |
489 | { |
491 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
490 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
492 | // auf Nullwerte kalibrieren |
491 | // auf Nullwerte kalibrieren |
493 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
492 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
494 | if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
493 | if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75) // Neutralwerte |
495 | { |
494 | { |
496 | if(++delay_neutral > 200) // nicht sofort |
495 | if(++delay_neutral > 200) // nicht sofort |
497 | { |
496 | { |
498 | GRN_OFF; |
497 | GRN_OFF; |
499 | MotorenEin = 0; |
498 | MotorenEin = 0; |
500 | delay_neutral = 0; |
499 | delay_neutral = 0; |
501 | modell_fliegt = 0; |
500 | modell_fliegt = 0; |
502 | if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > 70) |
501 | if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
503 | { |
502 | { |
504 | unsigned char setting=1; |
503 | unsigned char setting=1; |
505 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 1; |
504 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1; |
506 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 2; |
505 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2; |
507 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 3; |
506 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3; |
508 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 4; |
507 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4; |
509 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 5; |
508 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5; |
510 | SetActiveParamSet(setting); // aktiven Datensatz merken |
509 | SetActiveParamSet(setting); // aktiven Datensatz merken |
511 | } |
510 | } |
512 | if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
511 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
513 | { |
512 | { |
514 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
513 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
515 | } |
514 | } |
516 | ParamSet_ReadFromEEProm(GetActiveParamSet()); |
515 | ParamSet_ReadFromEEProm(GetActiveParamSet()); |
517 | SetNeutral(); |
516 | SetNeutral(); |
518 | Piep(GetActiveParamSet()); |
517 | Piep(GetActiveParamSet()); |
519 | } |
518 | } |
520 | } |
519 | } |
521 | else |
520 | else |
522 | if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
521 | if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75) // ACC Neutralwerte speichern |
523 | { |
522 | { |
524 | if(++delay_neutral > 200) // nicht sofort |
523 | if(++delay_neutral > 200) // nicht sofort |
525 | { |
524 | { |
526 | GRN_OFF; |
525 | GRN_OFF; |
527 | SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen |
526 | SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen |
Line 541... | Line 540... | ||
541 | else delay_neutral = 0; |
540 | else delay_neutral = 0; |
542 | } |
541 | } |
543 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
542 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
544 | // Gas ist unten |
543 | // Gas ist unten |
545 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
544 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
546 | if(PPM_in[ParamSet.Kanalbelegung[K_GAS]] < 35-120) |
545 | if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 35-120) |
547 | { |
546 | { |
548 | // Starten |
547 | // Starten |
549 | if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75) |
548 | if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] < -75) |
550 | { |
549 | { |
551 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
550 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
552 | // Einschalten |
551 | // Einschalten |
553 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
552 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
554 | if(++delay_einschalten > 200) |
553 | if(++delay_einschalten > 200) |
Line 570... | Line 569... | ||
570 | else delay_einschalten = 0; |
569 | else delay_einschalten = 0; |
571 | //Auf Neutralwerte setzen |
570 | //Auf Neutralwerte setzen |
572 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
571 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
573 | // Auschalten |
572 | // Auschalten |
574 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
573 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
575 | if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75) |
574 | if(PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 75) |
576 | { |
575 | { |
577 | if(++delay_ausschalten > 200) // nicht sofort |
576 | if(++delay_ausschalten > 200) // nicht sofort |
578 | { |
577 | { |
579 | MotorenEin = 0; |
578 | MotorenEin = 0; |
580 | delay_ausschalten = 200; |
579 | delay_ausschalten = 200; |
Line 591... | Line 590... | ||
591 | if(!NewPpmData-- || Notlandung) |
590 | if(!NewPpmData-- || Notlandung) |
592 | { |
591 | { |
593 | int tmp_int; |
592 | int tmp_int; |
594 | static int stick_nick,stick_roll; |
593 | static int stick_nick,stick_roll; |
595 | ParameterZuordnung(); |
594 | ParameterZuordnung(); |
596 | StickNick = (StickNick * 3 + PPM_in[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_P) / 4; |
595 | StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4; |
597 | StickNick += PPM_diff[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_D; |
596 | StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D; |
598 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_P) / 4; |
597 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
599 | StickRoll += PPM_diff[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_D; |
598 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
600 | 599 | ||
601 | StickGier = -PPM_in[ParamSet.Kanalbelegung[K_GIER]]; |
600 | StickGier = -PPM_in[ParamSet.ChannelAssignment[CH_GIER]]; |
602 | StickGas = PPM_in[ParamSet.Kanalbelegung[K_GAS]] + 120; |
601 | StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120; |
603 | 602 | ||
604 | if(abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]) > MaxStickNick) |
603 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) > MaxStickNick) |
605 | MaxStickNick = abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
604 | MaxStickNick = abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]); else MaxStickNick--; |
606 | if(abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
605 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
607 | MaxStickRoll = abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
606 | MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
608 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
607 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
Line 609... | Line 608... | ||
609 | 608 | ||
610 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
609 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
Line 646... | Line 645... | ||
646 | if(GyroFaktor < 0) GyroFaktor = 0; |
645 | if(GyroFaktor < 0) GyroFaktor = 0; |
647 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
646 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
648 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
647 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
649 | // Looping? |
648 | // Looping? |
650 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
649 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
651 | if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
650 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Links = 1; |
652 | else |
651 | else |
653 | { |
652 | { |
654 | { |
653 | { |
655 | if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Links = 0; |
654 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Links = 0; |
656 | } |
655 | } |
657 | } |
656 | } |
658 | if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
657 | if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Rechts = 1; |
659 | else |
658 | else |
660 | { |
659 | { |
661 | if(Looping_Rechts) // Hysterese |
660 | if(Looping_Rechts) // Hysterese |
662 | { |
661 | { |
663 | if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Rechts = 0; |
662 | if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Rechts = 0; |
664 | } |
663 | } |
665 | } |
664 | } |
Line 666... | Line 665... | ||
666 | 665 | ||
667 | if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
666 | if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Oben = 1; |
668 | else |
667 | else |
669 | { |
668 | { |
670 | if(Looping_Oben) // Hysterese |
669 | if(Looping_Oben) // Hysterese |
671 | { |
670 | { |
672 | if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Oben = 0; |
671 | if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Oben = 0; |
673 | } |
672 | } |
674 | } |
673 | } |
675 | if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
674 | if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Unten = 1; |
676 | else |
675 | else |
677 | { |
676 | { |
678 | if(Looping_Unten) // Hysterese |
677 | if(Looping_Unten) // Hysterese |
679 | { |
678 | { |
680 | if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Unten = 0; |
679 | if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Unten = 0; |
681 | } |
680 | } |
Line 682... | Line 681... | ||
682 | } |
681 | } |
683 | 682 | ||
Line 743... | Line 742... | ||
743 | if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
742 | if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
744 | { |
743 | { |
745 | tmp_long /= 3; |
744 | tmp_long /= 3; |
746 | tmp_long2 /= 3; |
745 | tmp_long2 /= 3; |
747 | } |
746 | } |
748 | if(abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25) |
747 | if(abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25) |
749 | { |
748 | { |
750 | tmp_long /= 3; |
749 | tmp_long /= 3; |
751 | tmp_long2 /= 3; |
750 | tmp_long2 /= 3; |
752 | } |
751 | } |
Line 783... | Line 782... | ||
783 | ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich; |
782 | ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich; |
Line 784... | Line 783... | ||
784 | 783 | ||
785 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
784 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
Line 786... | Line 785... | ||
786 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
785 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
787 | 786 | ||
788 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25)) |
787 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_GIER]]) > 25)) |
789 | { |
788 | { |
790 | LageKorrekturNick /= 2; |
789 | LageKorrekturNick /= 2; |
Line 847... | Line 846... | ||
847 | LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; |
846 | LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; |
848 | } |
847 | } |
849 | else last_n_n = 1; |
848 | else last_n_n = 1; |
850 | } else last_n_n = 0; |
849 | } else last_n_n = 0; |
851 | } else cnt = 0; |
850 | } else cnt = 0; |
852 | if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp; |
851 | if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
853 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
852 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
854 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
853 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
Line 855... | Line 854... | ||
855 | 854 | ||
856 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
855 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 884... | Line 883... | ||
884 | } else |
883 | } else |
885 | { |
884 | { |
886 | cnt = 0; |
885 | cnt = 0; |
887 | } |
886 | } |
Line 888... | Line 887... | ||
888 | 887 | ||
889 | if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp; |
888 | if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
890 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
889 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
891 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
890 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
892 | DebugOut.Analog[27] = ausgleichRoll; |
891 | DebugOut.Analog[27] = ausgleichRoll; |
893 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
892 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
Line 918... | Line 917... | ||
918 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
917 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
919 | // Gieren |
918 | // Gieren |
920 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
919 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
921 | if(abs(StickGier) > 20) // war 35 |
920 | if(abs(StickGier) > 20) // war 35 |
922 | { |
921 | { |
923 | if(!(ParamSet.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
922 | if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) NeueKompassRichtungMerken = 1; |
924 | } |
923 | } |
925 | tmp_int = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
924 | tmp_int = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
926 | tmp_int += (ParamSet.Gier_P * StickGier) / 4; |
925 | tmp_int += (ParamSet.Gier_P * StickGier) / 4; |
927 | sollGier = tmp_int; |
926 | sollGier = tmp_int; |
928 | Mess_Integral_Gier -= tmp_int; |
927 | Mess_Integral_Gier -= tmp_int; |
Line 930... | Line 929... | ||
930 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
929 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 931... | Line 930... | ||
931 | 930 | ||
932 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
931 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
933 | // Kompass |
932 | // Kompass |
934 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
933 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
935 | if(KompassValue && (ParamSet.GlobalConfig & CFG_KOMPASS_AKTIV)) |
934 | if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)) |
936 | { |
935 | { |
937 | int w,v; |
936 | int w,v; |
938 | static int SignalSchlecht = 0; |
937 | static int SignalSchlecht = 0; |
939 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
938 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
Line 1026... | Line 1025... | ||
1026 | // Höhenregelung |
1025 | // Höhenregelung |
1027 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1026 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1028 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1027 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1029 | //OCR0B = 180 - (Poti1 + 120) / 4; |
1028 | //OCR0B = 180 - (Poti1 + 120) / 4; |
1030 | //DruckOffsetSetting = OCR0B; |
1029 | //DruckOffsetSetting = OCR0B; |
1031 | if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1030 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung |
1032 | { |
1031 | { |
1033 | int tmp_int; |
1032 | int tmp_int; |
1034 | if(ParamSet.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1033 | if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) // Regler wird über Schalter gesteuert |
1035 | { |
1034 | { |
1036 | if(Parameter_MaxHoehe < 50) |
1035 | if(Parameter_MaxHoehe < 50) |
1037 | { |
1036 | { |
1038 | SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
1037 | SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
1039 | HoehenReglerAktiv = 0; |
1038 | HoehenReglerAktiv = 0; |