Rev 1914 | Rev 1921 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1914 | Rev 1916 | ||
---|---|---|---|
Line 91... | Line 91... | ||
91 | int ErsatzKompassInGrad; // Kompasswert in Grad |
91 | int ErsatzKompassInGrad; // Kompasswert in Grad |
92 | int GierGyroFehler = 0; |
92 | int GierGyroFehler = 0; |
93 | char GyroFaktor,GyroFaktorGier; |
93 | char GyroFaktor,GyroFaktorGier; |
94 | char IntegralFaktor,IntegralFaktorGier; |
94 | char IntegralFaktor,IntegralFaktorGier; |
95 | int DiffNick,DiffRoll; |
95 | int DiffNick,DiffRoll; |
- | 96 | int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
|
96 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
97 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
97 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
98 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
98 | volatile unsigned char SenderOkay = 0; |
99 | volatile unsigned char SenderOkay = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
100 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
100 | char MotorenEin = 0,StartTrigger = 0; |
101 | char MotorenEin = 0,StartTrigger = 0; |
Line 110... | Line 111... | ||
110 | int Ki = 10300 / 33; |
111 | int Ki = 10300 / 33; |
111 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
112 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
112 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
113 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
Line 113... | Line 114... | ||
113 | 114 | ||
114 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
115 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
115 | unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
116 | unsigned char Parameter_HoehenSchalter = 251; // Wert : 0-250 |
116 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
117 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
117 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
118 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
118 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
119 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
119 | unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250 |
120 | unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250 |
Line 152... | Line 153... | ||
152 | unsigned char Parameter_NaviGpsACC; |
153 | unsigned char Parameter_NaviGpsACC; |
153 | unsigned char Parameter_NaviOperatingRadius; |
154 | unsigned char Parameter_NaviOperatingRadius; |
154 | unsigned char Parameter_NaviWindCorrection; |
155 | unsigned char Parameter_NaviWindCorrection; |
155 | unsigned char Parameter_NaviSpeedCompensation; |
156 | unsigned char Parameter_NaviSpeedCompensation; |
156 | unsigned char Parameter_ExternalControl; |
157 | unsigned char Parameter_ExternalControl; |
- | 158 | unsigned char Parameter_GlobalConfig; |
|
- | 159 | unsigned char Parameter_ExtraConfig; |
|
157 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
160 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
158 | unsigned char CareFree = 0; |
161 | unsigned char CareFree = 0; |
159 | 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 |
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 |
Line 160... | Line 163... | ||
160 | 163 | ||
Line 407... | Line 410... | ||
407 | 410 | ||
408 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
411 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
409 | Mess_Integral_Gier += MesswertGier; |
412 | Mess_Integral_Gier += MesswertGier; |
410 | ErsatzKompass += MesswertGier; |
413 | ErsatzKompass += MesswertGier; |
411 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
414 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
412 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
415 | if(!Looping_Nick && !Looping_Roll && (Parameter_GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
413 | { |
416 | { |
414 | tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
417 | tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
415 | tmpl3 *= Parameter_AchsKopplung2; //65 |
418 | tmpl3 *= Parameter_AchsKopplung2; //65 |
416 | tmpl3 /= 4096L; |
419 | tmpl3 /= 4096L; |
Line 504... | Line 507... | ||
504 | if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
507 | if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
505 | else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
508 | else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
506 | if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
509 | if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
507 | else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
510 | else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
Line 508... | Line 511... | ||
508 | 511 | ||
509 | if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
512 | if(Parameter_GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
510 | { |
513 | { |
511 | if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
514 | if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
512 | else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
515 | else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
513 | if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
516 | if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
Line 577... | Line 580... | ||
577 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
580 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
578 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
581 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
579 | CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3); |
582 | CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3); |
580 | CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4); |
583 | CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4); |
581 | CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5); |
584 | CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5); |
582 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
- | |
583 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
585 | CHK_POTI(Parameter_HoehenSchalter,EE_Parameter.MaxHoehe); |
584 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
586 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
585 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
587 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
586 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
588 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
587 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I); |
589 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I); |
588 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D); |
590 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D); |
Line 601... | Line 603... | ||
601 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
603 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
602 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
604 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
603 | CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1); |
605 | CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1); |
604 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
606 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
605 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
607 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
- | 608 | Parameter_GlobalConfig = EE_Parameter.GlobalConfig; |
|
- | 609 | Parameter_ExtraConfig = EE_Parameter.ExtraConfig; |
|
606 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
610 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
607 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
611 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
608 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
612 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
609 | Ki = 10300 / (Parameter_I_Faktor + 1); |
613 | Ki = 10300 / (Parameter_I_Faktor + 1); |
610 | MAX_GAS = EE_Parameter.Gas_Max; |
614 | MAX_GAS = EE_Parameter.Gas_Max; |
Line 671... | Line 675... | ||
671 | if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
675 | if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
Line 672... | Line 676... | ||
672 | 676 | ||
673 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
677 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
674 | // Empfang schlecht |
678 | // Empfang schlecht |
675 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
679 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
676 | if(SenderOkay < 100) |
680 | if(SenderOkay < 100 && !(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE)) |
677 | { |
681 | { |
678 | if(RcLostTimer) RcLostTimer--; |
682 | if(RcLostTimer) RcLostTimer--; |
679 | else |
683 | else |
680 | { |
684 | { |
Line 745... | Line 749... | ||
745 | else |
749 | else |
746 | { |
750 | { |
747 | ParamSet_ReadFromEEProm(GetActiveParamSet()); |
751 | ParamSet_ReadFromEEProm(GetActiveParamSet()); |
748 | LipoDetection(0); |
752 | LipoDetection(0); |
749 | LIBFC_ReceiverInit(EE_Parameter.Receiver); |
753 | LIBFC_ReceiverInit(EE_Parameter.Receiver); |
750 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
754 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
751 | { |
755 | { |
752 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
756 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
753 | } |
757 | } |
754 | ServoActive = 0; |
758 | ServoActive = 0; |
755 | SetNeutral(0); |
759 | SetNeutral(0); |
Line 897... | Line 901... | ||
897 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
901 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
898 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
902 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
899 | } |
903 | } |
900 | if(StickGas < 0) StickGas = 0; |
904 | if(StickGas < 0) StickGas = 0; |
Line 901... | Line 905... | ||
901 | 905 | ||
Line 902... | Line 906... | ||
902 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
906 | if(Parameter_GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
903 | 907 | ||
904 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
908 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
905 | { |
909 | { |
Line 963... | Line 967... | ||
963 | 967 | ||
964 | 968 | ||
965 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
969 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 970 | // Bei Empfangsausfall im Flug |
|
- | 971 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 972 | if(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE) |
|
- | 973 | { |
|
- | 974 | StickNick = -GPS_Nick; |
|
- | 975 | StickRoll = -GPS_Roll; |
|
- | 976 | StickGas = StickGasHover; |
|
- | 977 | Parameter_GlobalConfig &= ~(CFG_HEADING_HOLD | CFG_DREHRATEN_BEGRENZER); |
|
- | 978 | Parameter_GlobalConfig |= CFG_HOEHENREGELUNG | CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER | CFG_GPS_AKTIV; |
|
- | 979 | Parameter_ExtraConfig &= ~(CFG2_HEIGHT_LIMIT | CFG_LEARNABLE_CAREFREE | CFG2_VARIO_BEEP); |
|
- | 980 | Parameter_HoehenSchalter = 200; // switch on |
|
966 | // Bei Empfangsausfall im Flug |
981 | } |
967 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
982 | else |
968 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) |
983 | if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) |
969 | { |
984 | { |
970 | StickGier = 0; |
985 | StickGier = 0; |
Line 1212... | Line 1227... | ||
1212 | // Gieren |
1227 | // Gieren |
1213 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1228 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1214 | if(abs(StickGier) > 3) // war 15 |
1229 | if(abs(StickGier) > 3) // war 15 |
1215 | { |
1230 | { |
1216 | // KompassSignalSchlecht = 1000; |
1231 | // KompassSignalSchlecht = 1000; |
1217 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
1232 | if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX)) |
1218 | { |
1233 | { |
1219 | NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen |
1234 | NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen |
1220 | }; |
1235 | }; |
1221 | } |
1236 | } |
1222 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1237 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
Line 1228... | Line 1243... | ||
1228 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1243 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 1229... | Line 1244... | ||
1229 | 1244 | ||
1230 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1245 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1231 | // Kompass |
1246 | // Kompass |
1232 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1247 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1233 | if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1248 | if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) |
1234 | { |
1249 | { |
1235 | if(CalculateCompassTimer-- == 1) |
1250 | if(CalculateCompassTimer-- == 1) |
1236 | { |
1251 | { |
1237 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
1252 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
Line 1305... | Line 1320... | ||
1305 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1320 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1306 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1321 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1307 | if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1322 | if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1308 | GasMischanteil *= STICK_GAIN; |
1323 | GasMischanteil *= STICK_GAIN; |
1309 | // if height control is activated |
1324 | // if height control is activated |
1310 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1325 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1311 | { |
1326 | { |
1312 | #define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
1327 | #define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
1313 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
1328 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
Line 1314... | Line 1329... | ||
1314 | 1329 | ||
Line 1318... | Line 1333... | ||
1318 | #define OPA_OFFSET_STEP 10 |
1333 | #define OPA_OFFSET_STEP 10 |
1319 | #endif |
1334 | #endif |
1320 | int HCGas, HeightDeviation = 0,GasReduction = 0; |
1335 | int HCGas, HeightDeviation = 0,GasReduction = 0; |
1321 | static int HeightTrimming = 0; // rate for change of height setpoint |
1336 | static int HeightTrimming = 0; // rate for change of height setpoint |
1322 | static int FilterHCGas = 0; |
1337 | static int FilterHCGas = 0; |
1323 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
- | |
1324 | static unsigned long HoverGasFilter = 0; |
1338 | static unsigned long HoverGasFilter = 0; |
1325 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1339 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1326 | static signed char WaypointTrimming = 0; |
1340 | static signed char WaypointTrimming = 0; |
1327 | int CosAttitude; // for projection of hoover gas |
1341 | int CosAttitude; // for projection of hoover gas |
Line 1376... | Line 1390... | ||
1376 | VarioMeter = 0; |
1390 | VarioMeter = 0; |
1377 | BaroExpandActive--; |
1391 | BaroExpandActive--; |
1378 | } |
1392 | } |
Line 1379... | Line 1393... | ||
1379 | 1393 | ||
1380 | // if height control is activated by an rc channel |
1394 | // if height control is activated by an rc channel |
1381 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1395 | if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1382 | { // check if parameter is less than activation threshold |
1396 | { // check if parameter is less than activation threshold |
1383 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1397 | if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1384 | { //height control not active |
1398 | { //height control not active |
1385 | if(!delay--) |
1399 | if(!delay--) |
1386 | { |
1400 | { |
1387 | HoehenReglerAktiv = 0; // disable height control |
1401 | HoehenReglerAktiv = 0; // disable height control |
Line 1395... | Line 1409... | ||
1395 | delay = 200; |
1409 | delay = 200; |
1396 | } |
1410 | } |
1397 | } |
1411 | } |
1398 | else // no switchable height control |
1412 | else // no switchable height control |
1399 | { |
1413 | { |
1400 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1414 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1401 | HoehenReglerAktiv = 1; |
1415 | HoehenReglerAktiv = 1; |
1402 | } |
1416 | } |
Line 1403... | Line 1417... | ||
1403 | 1417 | ||
1404 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1418 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
Line 1415... | Line 1429... | ||
1415 | // start of height control algorithm |
1429 | // start of height control algorithm |
1416 | // the height control is only an attenuation of the actual gas stick. |
1430 | // the height control is only an attenuation of the actual gas stick. |
1417 | // I.e. it will work only if the gas stick is higher than the hover gas |
1431 | // I.e. it will work only if the gas stick is higher than the hover gas |
1418 | // and the hover height will be allways larger than height setpoint. |
1432 | // and the hover height will be allways larger than height setpoint. |
1419 | FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
1433 | FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
1420 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1434 | if((Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) || !(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1421 | { // old version |
1435 | { // old version |
1422 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1436 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1423 | HeightTrimming = 0; |
1437 | HeightTrimming = 0; |
1424 | // set both flags to indicate no vario mode |
1438 | // set both flags to indicate no vario mode |
1425 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1439 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
Line 1491... | Line 1505... | ||
1491 | //else |
1505 | //else |
1492 | if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied |
1506 | if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied |
1493 | else WaypointTrimming = 0; |
1507 | else WaypointTrimming = 0; |
1494 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1508 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1495 | HeightTrimming = 0; |
1509 | HeightTrimming = 0; |
1496 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1510 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1497 | if(!StartTrigger && HoehenWert > 50) |
1511 | if(!StartTrigger && HoehenWert > 50) |
1498 | { |
1512 | { |
1499 | StartTrigger = 1; |
1513 | StartTrigger = 1; |
1500 | } |
1514 | } |
1501 | } |
1515 | } |
Line 1509... | Line 1523... | ||
1509 | else SollHoehe += WaypointTrimming; |
1523 | else SollHoehe += WaypointTrimming; |
1510 | } |
1524 | } |
1511 | else |
1525 | else |
1512 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1526 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1513 | HeightTrimming = 0; |
1527 | HeightTrimming = 0; |
1514 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // max. 2m Unterschied |
1528 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-512), (HoehenWert+512)); // max. 5m Unterschied |
1515 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1529 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1516 | //update hoover gas stick value when setpoint is shifted |
1530 | //update hoover gas stick value when setpoint is shifted |
1517 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
1531 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
1518 | { |
1532 | { |
1519 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
1533 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
1520 | StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
1534 | StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
Line 1531... | Line 1545... | ||
1531 | else StickGasHover = 120; |
1545 | else StickGasHover = 120; |
1532 | HoverGas = GasMischanteil; |
1546 | HoverGas = GasMischanteil; |
1533 | } |
1547 | } |
1534 | HCGas = HoverGas; // take hover gas (neutral point) |
1548 | HCGas = HoverGas; // take hover gas (neutral point) |
1535 | } |
1549 | } |
1536 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1550 | if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1537 | { |
1551 | { |
1538 | // from this point the Heigth Control Algorithm is identical for both versions |
1552 | // from this point the Heigth Control Algorithm is identical for both versions |
1539 | if(BaroExpandActive) // baro range expanding active |
1553 | if(BaroExpandActive) // baro range expanding active |
1540 | { |
1554 | { |
1541 | HCGas = HoverGas; // hover while expanding baro adc range |
1555 | HCGas = HoverGas; // hover while expanding baro adc range |
Line 1555... | Line 1569... | ||
1555 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1569 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1556 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1570 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1557 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1571 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1558 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1572 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1559 | else |
1573 | else |
1560 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1574 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1561 | GasReduction += tmp_int; |
1575 | GasReduction += tmp_int; |
1562 | } // EOF no baro range expanding |
1576 | } // EOF no baro range expanding |
1563 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1577 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1564 | if(Parameter_Hoehe_ACC_Wirkung) |
1578 | if(Parameter_Hoehe_ACC_Wirkung) |
1565 | { |
1579 | { |
Line 1608... | Line 1622... | ||
1608 | // update height control gas averaging |
1622 | // update height control gas averaging |
1609 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1623 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1610 | // limit height control gas pd-control output |
1624 | // limit height control gas pd-control output |
1611 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1625 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1612 | // set GasMischanteil to HeightControlGasFilter |
1626 | // set GasMischanteil to HeightControlGasFilter |
1613 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1627 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
1614 | { // old version |
1628 | { // old version |
1615 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1629 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1616 | GasMischanteil = FilterHCGas; |
1630 | GasMischanteil = FilterHCGas; |
1617 | } |
1631 | } |
1618 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1632 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
Line 1677... | Line 1691... | ||
1677 | { |
1691 | { |
1678 | StartTrigger = 0; |
1692 | StartTrigger = 0; |
1679 | HoverGasFilter = 0; |
1693 | HoverGasFilter = 0; |
1680 | HoverGas = 0; |
1694 | HoverGas = 0; |
1681 | } |
1695 | } |
1682 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1696 | }// EOF Parameter_GlobalConfig & CFG_HEIGHT_CONTROL |
1683 | else |
1697 | else |
1684 | { |
1698 | { |
1685 | // set undefined state to indicate vario off |
1699 | // set undefined state to indicate vario off |
1686 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1700 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1687 | } // EOF no height control |
1701 | } // EOF no height control |