Rev 1933 | Rev 1937 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1933 | Rev 1936 | ||
---|---|---|---|
Line 155... | Line 155... | ||
155 | unsigned char Parameter_NaviWindCorrection; |
155 | unsigned char Parameter_NaviWindCorrection; |
156 | unsigned char Parameter_NaviSpeedCompensation; |
156 | unsigned char Parameter_NaviSpeedCompensation; |
157 | unsigned char Parameter_ExternalControl; |
157 | unsigned char Parameter_ExternalControl; |
158 | unsigned char Parameter_GlobalConfig; |
158 | unsigned char Parameter_GlobalConfig; |
159 | unsigned char Parameter_ExtraConfig; |
159 | unsigned char Parameter_ExtraConfig; |
- | 160 | unsigned char Parameter_MaximumAltitude = 40; |
|
160 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
161 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
161 | unsigned char CareFree = 0; |
162 | unsigned char CareFree = 0; |
162 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps |
163 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps |
Line 163... | Line 164... | ||
163 | 164 | ||
Line 608... | Line 609... | ||
608 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
609 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
609 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
610 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
610 | CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1); |
611 | CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1); |
611 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
612 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
612 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
613 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
- | 614 | CHK_POTI(Parameter_MaximumAltitude,EE_Parameter.MaxAltitude); |
|
613 | Parameter_GlobalConfig = EE_Parameter.GlobalConfig; |
615 | Parameter_GlobalConfig = EE_Parameter.GlobalConfig; |
614 | Parameter_ExtraConfig = EE_Parameter.ExtraConfig; |
616 | Parameter_ExtraConfig = EE_Parameter.ExtraConfig; |
615 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
617 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
616 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
618 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
617 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
619 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
Line 646... | Line 648... | ||
646 | beeptime = 15000; |
648 | beeptime = 15000; |
647 | BeepMuster = 0xA400; |
649 | BeepMuster = 0xA400; |
648 | CareFree = 0; |
650 | CareFree = 0; |
649 | } |
651 | } |
650 | if(CareFree) { FC_StatusFlags2 |= FC_STATUS2_CAREFREE; if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} else FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
652 | if(CareFree) { FC_StatusFlags2 |= FC_STATUS2_CAREFREE; if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} else FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
- | 653 | // Limit the maximum Altitude |
|
- | 654 | if(Parameter_MaximumAltitude) if(SollHoehe/100 > Parameter_MaximumAltitude) SollHoehe = (long) Parameter_MaximumAltitude * 100L; |
|
651 | } |
655 | } |
Line 652... | Line 656... | ||
652 | 656 | ||
653 | //############################################################################ |
657 | //############################################################################ |
654 | // |
658 | // |
Line 1022... | Line 1026... | ||
1022 | 1026 | ||
1023 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1027 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1024 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1028 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1025 | { |
1029 | { |
- | 1030 | long tmp_long, tmp_long2; |
|
- | 1031 | ||
- | 1032 | //if(Poti3 < 120) FromNaviCtrl_Value.Kalman_K = Poti3/2; |
|
- | 1033 | //DebugOut.Analog[25] = FromNaviCtrl_Value.Kalman_K; |
|
- | 1034 | //DebugOut.Analog[26] = FromNaviCtrl_Value.Kalman_MaxFusion; |
|
- | 1035 | //DebugOut.Analog[16] = (Mittelwert_AccNick-FromNaviCtrl.AccErrorN) / 4; |
|
- | 1036 | //DebugOut.Analog[18] = (Mittelwert_AccRoll-FromNaviCtrl.AccErrorR) / 4; |
|
- | 1037 | //DebugOut.Analog[19] = -FromNaviCtrl.AccErrorR / 4; |
|
1026 | long tmp_long, tmp_long2; |
1038 | |
1027 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1039 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1028 | { |
1040 | { |
1029 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
1041 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
1030 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
1042 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
1031 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1043 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1032 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1044 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1033 | KompassFusion = FromNaviCtrl_Value.Kalman_K; |
1045 | KompassFusion = FromNaviCtrl_Value.Kalman_K; |
1034 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1046 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
Line 1269... | Line 1281... | ||
1269 | GierGyroFehler += fehler; |
1281 | GierGyroFehler += fehler; |
1270 | if(NeueKompassRichtungMerken) |
1282 | if(NeueKompassRichtungMerken) |
1271 | { |
1283 | { |
1272 | if(--NeueKompassRichtungMerken == 0) |
1284 | if(--NeueKompassRichtungMerken == 0) |
1273 | { |
1285 | { |
1274 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1286 | //--> ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1275 | KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1287 | KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1276 | } |
1288 | } |
1277 | } |
1289 | } |
1278 | } |
1290 | } |
1279 | // Kompass fusionieren |
1291 | // Kompass fusionieren |