Rev 1994 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1994 | Rev 1995 | ||
|---|---|---|---|
| Line 55... | Line 55... | ||
| 55 | 55 | ||
| 56 | #include "main.h" |
56 | #include "main.h" |
| 57 | #include "mymath.h" |
57 | #include "mymath.h" |
| Line -... | Line 58... | ||
| - | 58 | #include "isqrt.h" |
|
| - | 59 | ||
| - | 60 | ||
| - | 61 | //MartinW; added vars |
|
| - | 62 | unsigned char loop1, loop2, loop3; |
|
| - | 63 | unsigned char settingdest = 5; |
|
| - | 64 | int keynumber=-7; |
|
| - | 65 | ||
| - | 66 | unsigned short CurrentOffset = 0;/// |
|
| - | 67 | ||
| - | 68 | unsigned char pos1, pos2, pos3, pos4=0; |
|
| - | 69 | unsigned char Motors[8]; |
|
| - | 70 | unsigned char Motorsmax[8]; |
|
| - | 71 | unsigned short MotorsTmax; |
|
| - | 72 | unsigned char updatemotors=2; |
|
| - | 73 | ||
| - | 74 | //Panorama Trigger; |
|
| - | 75 | int degreeold =0; |
|
| - | 76 | int degreedivold =0; |
|
| - | 77 | int degreediv =0; |
|
| - | 78 | unsigned int panograd=0; |
|
| - | 79 | unsigned char panotrigger=0; |
|
| - | 80 | unsigned char calibration_done = 0; |
|
| - | 81 | ///MartinW; added vars END |
|
| - | 82 | ||
| 58 | #include "isqrt.h" |
83 | |
| 59 | 84 | ||
| 60 | unsigned char h,m,s; |
85 | unsigned char h,m,s; |
| 61 | unsigned int BaroExpandActive = 0; |
86 | unsigned int BaroExpandActive = 0; |
| 62 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
87 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
| Line 71... | Line 96... | ||
| 71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
96 | long IntegralRoll = 0,IntegralRoll2 = 0; |
| 72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
97 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| 73 | long Integral_Gier = 0; |
98 | long Integral_Gier = 0; |
| 74 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
99 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
| 75 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
100 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
| 76 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
101 | //long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; ///MartinW so war es |
| - | 102 | long Mess_Integral_Gier = 0; ///MartinW: Mess_Integral_Gier2 unbenutzt |
|
| - | 103 | ||
| 77 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
104 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
| 78 | long SummeNick=0,SummeRoll=0; |
105 | long SummeNick=0,SummeRoll=0; |
| 79 | volatile long Mess_Integral_Hoch = 0; |
106 | volatile long Mess_Integral_Hoch = 0; |
| 80 | int KompassValue = -1; |
107 | int KompassValue = -1; |
| 81 | int KompassSollWert = 0; |
108 | int KompassSollWert = 0; |
| Line 195... | Line 222... | ||
| 195 | DebugOut.Analog[11] = ErsatzKompassInGrad; |
222 | DebugOut.Analog[11] = ErsatzKompassInGrad; |
| 196 | DebugOut.Analog[12] = Motor[0].SetPoint; |
223 | DebugOut.Analog[12] = Motor[0].SetPoint; |
| 197 | DebugOut.Analog[13] = Motor[1].SetPoint; |
224 | DebugOut.Analog[13] = Motor[1].SetPoint; |
| 198 | DebugOut.Analog[14] = Motor[2].SetPoint; |
225 | DebugOut.Analog[14] = Motor[2].SetPoint; |
| 199 | DebugOut.Analog[15] = Motor[3].SetPoint; |
226 | DebugOut.Analog[15] = Motor[3].SetPoint; |
| - | 227 | ||
| - | 228 | ///MartinW added Debug ouputs |
|
| - | 229 | DebugOut.Analog[16] = Motor[4].SetPoint; |
|
| - | 230 | DebugOut.Analog[17] = Motor[5].SetPoint; |
|
| - | 231 | ||
| - | 232 | DebugOut.Analog[18] = FromNC_AltitudeSpeed; /// |
|
| - | 233 | ||
| - | 234 | //DebugOut.Analog[18] = Motor[6].SetPoint; // v0.84a |
|
| - | 235 | //DebugOut.Analog[24] = Motor[6].SetPoint; // on v0.84a = SollHoehe/5 |
|
| - | 236 | DebugOut.Analog[25] = Motor[6].SetPoint; |
|
| - | 237 | DebugOut.Analog[26] = Motor[7].SetPoint; |
|
| - | 238 | //DebugOut.Analog[27] = Motor[9].SetPoint; // on v0.84a = KompassSollWert |
|
| - | 239 | ///MartinW added Debug ouputs END |
|
| - | 240 | ||
| 200 | DebugOut.Analog[20] = ServoNickValue; |
241 | DebugOut.Analog[20] = ServoNickValue; |
| 201 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
242 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
| 202 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
243 | DebugOut.Analog[23] = Capacity.RemainCapacity; |
| 203 | DebugOut.Analog[24] = SollHoehe/5; |
244 | DebugOut.Analog[24] = SollHoehe/5; |
| 204 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
245 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
| 205 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
246 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
| 206 | DebugOut.Analog[27] = KompassSollWert; |
247 | DebugOut.Analog[27] = KompassSollWert; |
| 207 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
248 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
| Line 369... | Line 410... | ||
| 369 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
410 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
| 370 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
411 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
| 371 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
412 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
| 372 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
413 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
| 373 | carefree_old = 70; |
414 | carefree_old = 70; |
| 374 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
415 | #if ((defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) && defined(WITH_HOTTMENU)) |
| - | 416 | #warning : "### with Hottmenu ###" |
|
| 375 | LIBFC_HoTT_Clear(); |
417 | LIBFC_HoTT_Clear(); |
| 376 | #endif |
418 | #endif |
| 377 | } |
419 | } |
| Line 809... | Line 851... | ||
| 809 | { |
851 | { |
| 810 | modell_fliegt = 1; |
852 | modell_fliegt = 1; |
| 811 | MotorenEin = 1; |
853 | MotorenEin = 1; |
| 812 | sollGier = 0; |
854 | sollGier = 0; |
| 813 | Mess_Integral_Gier = 0; |
855 | Mess_Integral_Gier = 0; |
| 814 | Mess_Integral_Gier2 = 0; |
856 | // Mess_Integral_Gier2 = 0; |
| 815 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
857 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
| 816 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
858 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
| 817 | Mess_IntegralNick2 = IntegralNick; |
859 | Mess_IntegralNick2 = IntegralNick; |
| 818 | Mess_IntegralRoll2 = IntegralRoll; |
860 | Mess_IntegralRoll2 = IntegralRoll; |
| 819 | SummeNick = 0; |
861 | SummeNick = 0; |
| Line 902... | Line 944... | ||
| 902 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
944 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
| Line 903... | Line 945... | ||
| 903 | 945 | ||
| 904 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
946 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 905 | //+ Analoge Steuerung per Seriell |
947 | //+ Analoge Steuerung per Seriell |
| - | 948 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 949 | #ifdef WITH_ExternControl /// MartinW memorysaving |
|
| - | 950 | #warning : "### with ExternControl ###" |
|
| 906 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
951 | |
| 907 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
952 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
| 908 | { |
953 | { |
| 909 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
954 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
| 910 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
955 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
| Line 928... | Line 973... | ||
| 928 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
973 | if(MaxStickRoll > 100) MaxStickRoll = 100; |
| 929 | } |
974 | } |
| 930 | else MaxStickRoll--; |
975 | else MaxStickRoll--; |
| 931 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
976 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;} |
| Line -... | Line 977... | ||
| - | 977 | ||
| - | 978 | #else |
|
| - | 979 | #warning : "### without ExternControl ###" |
|
| - | 980 | #endif |
|
| 932 | 981 | ||
| 933 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
982 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 934 | // Looping? |
983 | // Looping? |
| 935 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
984 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 936 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
985 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1; |