Rev 553 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 553 | Rev 565 | ||
|---|---|---|---|
| Line 83... | Line 83... | ||
| 83 | unsigned char MAX_GAS,MIN_GAS; |
83 | unsigned char MAX_GAS,MIN_GAS; |
| 84 | unsigned char Notlandung = 0; |
84 | unsigned char Notlandung = 0; |
| 85 | unsigned char HoehenReglerAktiv = 0; |
85 | unsigned char HoehenReglerAktiv = 0; |
| 86 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
86 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| Line 87... | Line -... | ||
| 87 | - | ||
| 88 | 87 | ||
| 89 | //Salvo 12.10.2007 |
88 | //Salvo 12.10.2007 |
| 90 | uint8_t magkompass_ok=0; |
89 | uint8_t magkompass_ok=0; |
| 91 | uint8_t gps_cmd = GPS_CMD_STOP; |
90 | uint8_t gps_cmd = GPS_CMD_STOP; |
| 92 | static int ubat_cnt =0; |
91 | static int ubat_cnt =0; |
| Line 396... | Line 395... | ||
| 396 | motor = 0; |
395 | motor = 0; |
| 397 | i2c_start(); |
396 | i2c_start(); |
| 398 | } |
397 | } |
| Line 399... | Line -... | ||
| 399 | - | ||
| 400 | 398 | ||
| 401 | 399 | ||
| 402 | //############################################################################ |
400 | //############################################################################ |
| 403 | // Trägt ggf. das Poti als Parameter ein |
401 | // Trägt ggf. das Poti als Parameter ein |
| 404 | void ParameterZuordnung(void) |
402 | void ParameterZuordnung(void) |
| Line 464... | Line 462... | ||
| 464 | ROT_ON; //led blitzen lassen |
462 | ROT_ON; //led blitzen lassen |
| 465 | } |
463 | } |
| 466 | //******PROVISORISCH*************** |
464 | //******PROVISORISCH*************** |
| 467 | GRN_ON; |
465 | GRN_ON; |
| Line 468... | Line -... | ||
| 468 | - | ||
| 469 | GRN_ON; |
466 | |
| 470 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
467 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 471 | // Gaswert ermitteln |
468 | // Gaswert ermitteln |
| 472 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
469 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 473 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
470 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
| 474 | if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; |
471 | if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; |
| 475 | //Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
472 | //Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
| 476 | // und dieser dann langsam zwangsweise reduziert |
473 | // und dieser dann langsam zwangsweise reduziert |
| 477 | if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
474 | if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
| 478 | { |
475 | { |
| 479 | if (ubat_cnt > 700) |
476 | if (ubat_cnt > 1000) |
| 480 | { |
477 | { |
| 481 | ubat_cnt = 0; |
478 | ubat_cnt = 0; |
| 482 | if (gas_actual > ((gas_mittel*12)/15)) gas_actual--; |
479 | if (gas_actual > ((gas_mittel*12)/15)) gas_actual--; |
| 483 | } |
480 | } |
| Line 590... | Line 587... | ||
| 590 | } |
587 | } |
| 591 | GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
588 | GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
| 592 | if (gps_home_position.status > 0 ) |
589 | if (gps_home_position.status > 0 ) |
| 593 | { |
590 | { |
| 594 | Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
591 | Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
| 595 | beeptime = 2000; |
592 | beeptime = 1000; |
| 596 | Delay_ms(500); |
593 | Delay_ms(500); |
| 597 | } |
594 | } |
| 598 | } |
595 | } |
| 599 | } |
596 | } |
| 600 | else |
597 | else |
| Line 631... | Line 628... | ||
| 631 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
628 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 632 | // Einschalten |
629 | // Einschalten |
| 633 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
630 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 634 | if(++delay_einschalten > 200) |
631 | if(++delay_einschalten > 200) |
| 635 | { |
632 | { |
| - | 633 | int n; |
|
| 636 | // Salvo 9.12.2007 |
634 | // Salvo 9.12.2007 |
| 637 | RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
635 | RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
| 638 | // Salvo End |
636 | // Salvo End |
| 639 | delay_einschalten = 200; |
637 | delay_einschalten = 200; |
| 640 | modell_fliegt = 1; |
638 | modell_fliegt = 1; |
| Line 646... | Line 644... | ||
| 646 | Mess_IntegralRoll = 0; |
644 | Mess_IntegralRoll = 0; |
| 647 | Mess_IntegralNick2 = IntegralNick; |
645 | Mess_IntegralNick2 = IntegralNick; |
| 648 | Mess_IntegralRoll2 = IntegralRoll; |
646 | Mess_IntegralRoll2 = IntegralRoll; |
| 649 | SummeNick = 0; |
647 | SummeNick = 0; |
| 650 | SummeRoll = 0; |
648 | SummeRoll = 0; |
| - | 649 | n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
|
| 651 | } |
650 | } |
| 652 | } |
651 | } |
| 653 | else delay_einschalten = 0; |
652 | else delay_einschalten = 0; |
| 654 | //Auf Neutralwerte setzen |
653 | //Auf Neutralwerte setzen |
| 655 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
654 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 932... | Line 931... | ||
| 932 | //Salvo 11.12.2007 |
931 | //Salvo 11.12.2007 |
| 933 | w = (abs(Mittelwert_AccNick)); |
932 | w = (abs(Mittelwert_AccNick)); |
| 934 | v = (abs(Mittelwert_AccRoll)); |
933 | v = (abs(Mittelwert_AccRoll)); |
| 935 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen |
934 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen |
| 936 | { |
935 | { |
| 937 | - | ||
| 938 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
936 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
| 939 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
937 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
| 940 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
938 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
| 941 | } |
939 | } |
| 942 | //Salvo End |
940 | //Salvo End |
| Line 1003... | Line 1001... | ||
| 1003 | MittelIntegralNick2 = 0; |
1001 | MittelIntegralNick2 = 0; |
| 1004 | MittelIntegralRoll2 = 0; |
1002 | MittelIntegralRoll2 = 0; |
| 1005 | ZaehlMessungen = 0; |
1003 | ZaehlMessungen = 0; |
| Line 1006... | Line -... | ||
| 1006 | - | ||
| 1007 | - | ||
| 1008 | // Salvo 6.10.2007 |
- | |
| 1009 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
- | |
| 1010 | //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
- | |
| 1011 | if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF) |
- | |
| 1012 | && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0)) |
- | |
| 1013 | { |
- | |
| 1014 | if (Parameter_MaxHoehe > 200) |
- | |
| 1015 | { |
- | |
| 1016 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
- | |
| 1017 | else gps_cmd = GPS_CMD_REQ_HOME; |
- | |
| 1018 | n = GPS_CRTL(gps_cmd); |
- | |
| 1019 | } |
- | |
| 1020 | else |
- | |
| 1021 | { |
- | |
| 1022 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
- | |
| 1023 | else gps_cmd = GPS_CMD_REQ_HOLD; |
- | |
| 1024 | n= GPS_CRTL(gps_cmd); |
- | |
| 1025 | } |
- | |
| 1026 | } |
- | |
| 1027 | else |
- | |
| 1028 | { |
- | |
| 1029 | if (gps_cmd != GPS_CMD_STOP) |
- | |
| 1030 | { |
- | |
| 1031 | gps_cmd = GPS_CMD_STOP; |
- | |
| 1032 | n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
- | |
| 1033 | } |
1004 | |
| Line 1034... | Line 1005... | ||
| 1034 | } |
1005 | |
| 1035 | } // Ende Abgleich |
1006 | } // Ende Abgleich |
| 1036 | 1007 | ||
| Line 1053... | Line 1024... | ||
| 1053 | w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT); |
1024 | w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT); |
| 1054 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
1025 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
| 1055 | if (v <-180) v +=360; // Uberlaufkorrektur |
1026 | if (v <-180) v +=360; // Uberlaufkorrektur |
| 1056 | if (v > 180) v -=360; // Uberlaufkorrektur |
1027 | if (v > 180) v -=360; // Uberlaufkorrektur |
| Line 1057... | Line 1028... | ||
| 1057 | 1028 | ||
| Line 1058... | Line 1029... | ||
| 1058 | w = w -v; //Differenz Gyro zu Kompass ist der Driftfehler |
1029 | v = w -v; //Differenz Gyro zu Kompass ist der Driftfehler |
| 1059 | 1030 | ||
| 1060 | #define GIER_COMP_MAX 4 |
1031 | #define GIER_COMP_MAX 4 |
| 1061 | if (w > GIER_COMP_MAX) w= GIER_COMP_MAX; |
1032 | if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
| 1062 | if (w < -GIER_COMP_MAX) w= - GIER_COMP_MAX; |
1033 | if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
| 1063 | if (!(w == 0)) |
1034 | if (abs(w) > 1) |
| 1064 | { |
1035 | { |
| 1065 | GyroGier_Comp = 0; |
1036 | GyroGier_Comp = 0; |
| 1066 | gyrogier_kompass = KompassValue; // Kompasswert merken |
1037 | gyrogier_kompass = KompassValue; // Kompasswert merken |
| 1067 | AdNeutralGier -= w; |
1038 | AdNeutralGier -= v; |
| 1068 | } |
1039 | } |
| 1069 | } |
1040 | } |
| 1070 | } |
1041 | } |
| Line 1106... | Line 1077... | ||
| 1106 | } |
1077 | } |
| 1107 | Kompass_Value_Old = KompassValue; |
1078 | Kompass_Value_Old = KompassValue; |
| 1108 | } |
1079 | } |
| 1109 | // Salvo End ************************* |
1080 | // Salvo End ************************* |
| Line -... | Line 1081... | ||
| - | 1081 | ||
| - | 1082 | // Salvo 6.10.2007 |
|
| - | 1083 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
|
| - | 1084 | //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
|
| - | 1085 | if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF) |
|
| - | 1086 | && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0) && (GasMischanteil > 30)) |
|
| - | 1087 | { |
|
| - | 1088 | if (Parameter_MaxHoehe > 200) |
|
| - | 1089 | { |
|
| - | 1090 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
| - | 1091 | else gps_cmd = GPS_CMD_REQ_HOME; |
|
| - | 1092 | n = GPS_CRTL(gps_cmd); |
|
| - | 1093 | } |
|
| - | 1094 | else |
|
| - | 1095 | { |
|
| - | 1096 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
| - | 1097 | else gps_cmd = GPS_CMD_REQ_HOLD; |
|
| - | 1098 | n= GPS_CRTL(gps_cmd); |
|
| - | 1099 | } |
|
| - | 1100 | } |
|
| - | 1101 | else |
|
| - | 1102 | { |
|
| - | 1103 | if (gps_cmd != GPS_CMD_STOP) |
|
| - | 1104 | { |
|
| - | 1105 | gps_cmd = GPS_CMD_STOP; |
|
| - | 1106 | n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
|
| - | 1107 | } |
|
| - | 1108 | } |
|
| - | 1109 | if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 10) LED_J16_OFF; //led im GPS Mode schnell blinken lassen |
|
| Line 1110... | Line 1110... | ||
| 1110 | 1110 | ||
| 1111 | 1111 | ||
| 1112 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1112 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1113 | // Gieren |
1113 | // Gieren |
| Line 1145... | Line 1145... | ||
| 1145 | if(w > 0) |
1145 | if(w > 0) |
| 1146 | { |
1146 | { |
| 1147 | // Salvo Kompasssteuerung ********************** |
1147 | // Salvo Kompasssteuerung ********************** |
| 1148 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
1148 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| 1149 | // Salvo End |
1149 | // Salvo End |
| 1150 | } |
1150 | } |
| 1151 | - | ||
| 1152 | } |
1151 | } |
| Line 1153... | Line 1152... | ||
| 1153 | 1152 | ||
| Line 1154... | Line 1153... | ||
| 1154 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1153 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1155 | 1154 | ||
| 1156 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1155 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1157 | // Debugwerte zuordnen |
1156 | // Debugwerte zuordnen |
| 1158 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1157 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 1158 | if(!TimerWerteausgabe--) |
|
| Line 1159... | Line 1159... | ||
| 1159 | if(!TimerWerteausgabe--) |
1159 | { |
| 1160 | { |
1160 | TimerWerteausgabe = 24; |
| 1161 | 1161 | ||
| 1162 | // Salvo 13.12.2007 Beleuchtung steuern |
1162 | // Salvo 13.12.2007 Beleuchtung steuern |
| 1163 | if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
1163 | if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
| Line 1164... | Line -... | ||
| 1164 | else if (MotorenEin) LED_J16_ON; |
- | |
| 1165 | else LED_J16_OFF; |
1164 | else if (MotorenEin) LED_J16_ON; |
| 1166 | // Salvo End |
1165 | else LED_J16_OFF; |
| 1167 | 1166 | // Salvo End |
|
| 1168 | TimerWerteausgabe = 24; |
1167 | |
| 1169 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
1168 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
| Line 1190... | Line 1189... | ||
| 1190 | // DebugOut.Analog[11] = GPS_hdng_rel_2trgt; |
1189 | // DebugOut.Analog[11] = GPS_hdng_rel_2trgt; |
| 1191 | DebugOut.Analog[10] = Parameter_UserParam1; |
1190 | DebugOut.Analog[10] = Parameter_UserParam1; |
| 1192 | DebugOut.Analog[11] = Parameter_UserParam3; |
1191 | DebugOut.Analog[11] = Parameter_UserParam3; |
| 1193 | DebugOut.Analog[24] = GPS_Nick; |
1192 | DebugOut.Analog[24] = GPS_Nick; |
| 1194 | DebugOut.Analog[25] = GPS_Roll; |
1193 | DebugOut.Analog[25] = GPS_Roll; |
| 1195 | DebugOut.Analog[26] = gps_rel_act_position.utm_east/10; //in m ausgeben |
1194 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
| 1196 | DebugOut.Analog[27] = gps_rel_act_position.utm_north/10; |
1195 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
| 1197 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt/10; |
1196 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
| 1198 | DebugOut.Analog[29] = gps_sub_state+(20*gps_cmd); |
1197 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
| - | 1198 | DebugOut.Analog[30] = GPS_dist_2trgt; |
|
| 1199 | DebugOut.Analog[31] = (int) GyroGier_Comp; |
1199 | DebugOut.Analog[31] = (int) GyroGier_Comp; |
| 1200 | - | ||
| 1201 | /* DebugOut.Analog[16] = motor_rx[0]; |
1200 | /* DebugOut.Analog[16] = motor_rx[0]; |
| 1202 | DebugOut.Analog[17] = motor_rx[1]; |
1201 | DebugOut.Analog[17] = motor_rx[1]; |
| 1203 | DebugOut.Analog[18] = motor_rx[2]; |
1202 | DebugOut.Analog[18] = motor_rx[2]; |
| 1204 | DebugOut.Analog[19] = motor_rx[3]; |
1203 | DebugOut.Analog[19] = motor_rx[3]; |
| 1205 | DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
1204 | DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |