Rev 1085 | Rev 1091 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1085 | Rev 1086 | ||
---|---|---|---|
Line 49... | Line 49... | ||
49 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
49 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
51 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
51 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + POSSIBILITY OF SUCH DAMAGE. |
52 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
53 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 54 | // Aenderungen von Peter Muehlenbrock ("Salvo") Stand 9.11.2008 |
|
- | 55 | /* |
|
- | 56 | Driftkompensation fuer Gyros verbessert |
|
- | 57 | Linearsensor optional mit fixem Neutralwert |
|
- | 58 | Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion |
|
- | 59 | */ |
|
Line 54... | Line 60... | ||
54 | 60 | ||
55 | #include "main.h" |
61 | #include "main.h" |
Line 56... | Line 62... | ||
56 | #include "eeprom.c" |
62 | #include "eeprom.c" |
Line 83... | Line 89... | ||
83 | unsigned char TrichterFlug = 0; |
89 | unsigned char TrichterFlug = 0; |
84 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
90 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
85 | long ErsatzKompass; |
91 | long ErsatzKompass; |
86 | int ErsatzKompassInGrad; // Kompasswert in Grad |
92 | int ErsatzKompassInGrad; // Kompasswert in Grad |
87 | int GierGyroFehler = 0; |
93 | int GierGyroFehler = 0; |
- | 94 | //Salvo 12.10.2007 |
|
- | 95 | uint8_t magkompass_ok=0; |
|
- | 96 | uint8_t gps_cmd = GPS_CMD_STOP; |
|
- | 97 | static int ubat_cnt =0; |
|
- | 98 | static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
|
- | 99 | int w,v; |
|
- | 100 | //Salvo End |
|
- | 101 | ||
- | 102 | //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation |
|
- | 103 | long GyroKomp_Int; |
|
- | 104 | long int GyroGier_Comp; |
|
- | 105 | int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
|
- | 106 | short int cnt_stickgier_zero =0; |
|
- | 107 | int gyrogier_kompass; |
|
- | 108 | //Salvo End |
|
- | 109 | ||
- | 110 | //Salvo 2.1.2008 Allgemeine Debugvariablen |
|
- | 111 | int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
|
- | 112 | //Salvo End |
|
88 | float GyroFaktor; |
113 | float GyroFaktor; |
89 | float IntegralFaktor; |
114 | float IntegralFaktor; |
90 | volatile int DiffNick,DiffRoll; |
115 | volatile int DiffNick,DiffRoll; |
91 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
116 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
92 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
117 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
Line 130... | Line 155... | ||
130 | unsigned char Parameter_NaviGpsGain; |
155 | unsigned char Parameter_NaviGpsGain; |
131 | unsigned char Parameter_NaviGpsP; |
156 | unsigned char Parameter_NaviGpsP; |
132 | unsigned char Parameter_NaviGpsI; |
157 | unsigned char Parameter_NaviGpsI; |
133 | unsigned char Parameter_NaviGpsD; |
158 | unsigned char Parameter_NaviGpsD; |
134 | unsigned char Parameter_NaviGpsACC; |
159 | unsigned char Parameter_NaviGpsACC; |
- | 160 | unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008 |
|
135 | unsigned char Parameter_NaviOperatingRadius; |
161 | unsigned char Parameter_NaviOperatingRadius; |
136 | unsigned char Parameter_NaviWindCorrection; |
162 | unsigned char Parameter_NaviWindCorrection; |
137 | unsigned char Parameter_NaviSpeedCompensation; |
163 | unsigned char Parameter_NaviSpeedCompensation; |
138 | unsigned char Parameter_ExternalControl; |
164 | unsigned char Parameter_ExternalControl; |
139 | struct mk_param_struct EE_Parameter; |
165 | struct mk_param_struct EE_Parameter; |
140 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
166 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
141 | int MaxStickNick = 0,MaxStickRoll = 0; |
167 | int MaxStickNick = 0,MaxStickRoll = 0; |
142 | unsigned int modell_fliegt = 0; |
168 | unsigned int modell_fliegt = 0; |
143 | unsigned char MikroKopterFlags = 0; |
169 | unsigned char MikroKopterFlags = 0; |
Line -... | Line 170... | ||
- | 170 | ||
- | 171 | //Salvo 2.1.2008 Allgemeine Debugvariablen |
|
- | 172 | int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
|
- | 173 | //Salvo End |
|
- | 174 | ||
- | 175 | ||
- | 176 | ||
144 | 177 | ||
145 | void Piep(unsigned char Anzahl) |
178 | void Piep(unsigned char Anzahl) |
146 | { |
179 | { |
147 | while(Anzahl--) |
180 | while(Anzahl--) |
148 | { |
181 | { |
Line 155... | Line 188... | ||
155 | //############################################################################ |
188 | //############################################################################ |
156 | // Nullwerte ermitteln |
189 | // Nullwerte ermitteln |
157 | void SetNeutral(void) |
190 | void SetNeutral(void) |
158 | //############################################################################ |
191 | //############################################################################ |
159 | { |
192 | { |
- | 193 | // Salvo 9.12.2007 |
|
- | 194 | RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
|
- | 195 | // Salvo End |
|
160 | NeutralAccX = 0; |
196 | NeutralAccX = 0; |
161 | NeutralAccY = 0; |
197 | NeutralAccY = 0; |
162 | NeutralAccZ = 0; |
198 | NeutralAccZ = 0; |
163 | AdNeutralNick = 0; |
199 | AdNeutralNick = 0; |
164 | AdNeutralRoll = 0; |
200 | AdNeutralRoll = 0; |
165 | AdNeutralGier = 0; |
201 | AdNeutralGier = 0; |
Line 218... | Line 254... | ||
218 | LED_Init(); |
254 | LED_Init(); |
219 | MikroKopterFlags |= FLAG_CALIBRATE; |
255 | MikroKopterFlags |= FLAG_CALIBRATE; |
220 | FromNaviCtrl_Value.Kalman_K = -1; |
256 | FromNaviCtrl_Value.Kalman_K = -1; |
221 | FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16; |
257 | FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16; |
222 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
258 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
- | 259 | ||
- | 260 | //Salvo 13.10.2007 Ersatzkompass und Gas |
|
- | 261 | GyroKomp_Int = KompassValue * GIER_GRAD_FAKTOR; //Neu ab 15.10.2008 |
|
- | 262 | // gas_mittel = 30; |
|
- | 263 | // gas_actual = gas_mittel; |
|
- | 264 | // Salvo End |
|
- | 265 | ||
223 | } |
266 | } |
Line 224... | Line 267... | ||
224 | 267 | ||
225 | //############################################################################ |
268 | //############################################################################ |
226 | // Bearbeitet die Messwerte |
269 | // Bearbeitet die Messwerte |
Line 232... | Line 275... | ||
232 | MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
275 | MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
233 | MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
276 | MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
234 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
277 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
Line 235... | Line 278... | ||
235 | 278 | ||
236 | //DebugOut.Analog[26] = MesswertNick; |
279 | //DebugOut.Analog[26] = MesswertNick; |
Line 237... | Line 280... | ||
237 | DebugOut.Analog[28] = MesswertRoll; |
280 | //DebugOut.Analog[28] = MesswertRoll; |
238 | 281 | ||
239 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
282 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
240 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
283 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
Line 246... | Line 289... | ||
246 | NaviAccRoll += AdWertAccRoll; |
289 | NaviAccRoll += AdWertAccRoll; |
247 | NaviCntAcc++; |
290 | NaviCntAcc++; |
248 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
291 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
249 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
292 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
250 | ErsatzKompass += MesswertGier; |
293 | ErsatzKompass += MesswertGier; |
- | 294 | //Salvo 12.11.2007 |
|
- | 295 | GyroKomp_Int += (long)MesswertGier; |
|
- | 296 | GyroGier_Comp += (long)MesswertGier; |
|
- | 297 | //Salvo End |
|
251 | Mess_Integral_Gier += MesswertGier; |
298 | Mess_Integral_Gier += MesswertGier; |
252 | // Mess_Integral_Gier2 += MesswertGier; |
299 | // Mess_Integral_Gier2 += MesswertGier; |
253 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
300 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
254 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
301 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
255 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
302 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
Line 436... | Line 483... | ||
436 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
483 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
437 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
484 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
438 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
485 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
439 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
486 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
Line 440... | Line 487... | ||
440 | 487 | ||
441 | // CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
488 | CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
442 | //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
489 | CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
443 | // CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
490 | CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
444 | // CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
491 | CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
445 | // CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
492 | CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
446 | // CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
493 | CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
447 | // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255); |
494 | // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255); |
448 | // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255); |
495 | // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255); |
Line 449... | Line 496... | ||
449 | // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255); |
496 | // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255); |
Line 473... | Line 520... | ||
473 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
520 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
474 | static int hoehenregler = 0; |
521 | static int hoehenregler = 0; |
475 | static char TimerWerteausgabe = 0; |
522 | static char TimerWerteausgabe = 0; |
476 | static char NeueKompassRichtungMerken = 0; |
523 | static char NeueKompassRichtungMerken = 0; |
477 | static long ausgleichNick, ausgleichRoll; |
524 | static long ausgleichNick, ausgleichRoll; |
478 | 525 | ||
479 | Mittelwert(); |
526 | Mittelwert(); |
- | 527 | //****** GPS Daten holen *************** |
|
- | 528 | short int n; |
|
- | 529 | if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout) |
|
- | 530 | n = Get_Rel_Position(); |
|
- | 531 | if (n == 0) |
|
- | 532 | { |
|
- | 533 | ROT_ON; //led blitzen lassen |
|
480 | 534 | } |
|
- | 535 | //******PROVISORISCH*************** |
|
481 | GRN_ON; |
536 | GRN_ON; |
482 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
537 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
483 | // Gaswert ermitteln |
538 | // Gaswert ermitteln |
484 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
539 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
485 | GasMischanteil = StickGas; |
540 | GasMischanteil = StickGas; |
Line 549... | Line 604... | ||
549 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
604 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
550 | { |
605 | { |
551 | if(++delay_neutral > 200) // nicht sofort |
606 | if(++delay_neutral > 200) // nicht sofort |
552 | { |
607 | { |
553 | GRN_OFF; |
608 | GRN_OFF; |
- | 609 | SetNeutral(); |
|
554 | MotorenEin = 0; |
610 | MotorenEin = 0; |
555 | delay_neutral = 0; |
611 | delay_neutral = 0; |
556 | modell_fliegt = 0; |
612 | modell_fliegt = 0; |
557 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
613 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
558 | { |
614 | { |
Line 577... | Line 633... | ||
577 | { |
633 | { |
578 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
634 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
579 | } |
635 | } |
580 | SetNeutral(); |
636 | SetNeutral(); |
581 | Piep(GetActiveParamSetNumber()); |
637 | Piep(GetActiveParamSetNumber()); |
- | 638 | GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
|
- | 639 | if (gps_home_position.status > 0 ) |
|
- | 640 | { |
|
- | 641 | Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
|
- | 642 | beeptime = 1000; |
|
- | 643 | Delay_ms(500); |
|
- | 644 | } |
|
582 | } |
645 | } |
583 | } |
646 | } |
584 | } |
647 | } |
585 | else |
648 | else |
586 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
649 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
Line 615... | Line 678... | ||
615 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
678 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
616 | // Einschalten |
679 | // Einschalten |
617 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
680 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
618 | if(++delay_einschalten > 200) |
681 | if(++delay_einschalten > 200) |
619 | { |
682 | { |
- | 683 | int n; |
|
- | 684 | // Salvo 9.12.2007 |
|
- | 685 | RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
|
- | 686 | // Salvo End |
|
620 | delay_einschalten = 200; |
687 | delay_einschalten = 200; |
621 | modell_fliegt = 1; |
688 | modell_fliegt = 1; |
622 | MotorenEin = 1; |
689 | MotorenEin = 1; |
623 | sollGier = 0; |
690 | sollGier = 0; |
624 | Mess_Integral_Gier = 0; |
691 | Mess_Integral_Gier = 0; |
Line 627... | Line 694... | ||
627 | Mess_IntegralRoll = 0; |
694 | Mess_IntegralRoll = 0; |
628 | Mess_IntegralNick2 = IntegralNick; |
695 | Mess_IntegralNick2 = IntegralNick; |
629 | Mess_IntegralRoll2 = IntegralRoll; |
696 | Mess_IntegralRoll2 = IntegralRoll; |
630 | SummeNick = 0; |
697 | SummeNick = 0; |
631 | SummeRoll = 0; |
698 | SummeRoll = 0; |
- | 699 | n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
|
- | 700 | ||
632 | MikroKopterFlags |= FLAG_START; |
701 | MikroKopterFlags |= FLAG_START; |
633 | } |
702 | } |
634 | } |
703 | } |
635 | else delay_einschalten = 0; |
704 | else delay_einschalten = 0; |
636 | //Auf Neutralwerte setzen |
705 | //Auf Neutralwerte setzen |
Line 639... | Line 708... | ||
639 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
708 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
640 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
709 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
641 | { |
710 | { |
642 | if(++delay_ausschalten > 200) // nicht sofort |
711 | if(++delay_ausschalten > 200) // nicht sofort |
643 | { |
712 | { |
- | 713 | // Salvo 9.12.2007 |
|
- | 714 | RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten |
|
- | 715 | // Salvo End |
|
- | 716 | ||
644 | MotorenEin = 0; |
717 | MotorenEin = 0; |
645 | delay_ausschalten = 200; |
718 | delay_ausschalten = 200; |
646 | modell_fliegt = 0; |
719 | modell_fliegt = 0; |
647 | } |
720 | } |
648 | } |
721 | } |
Line 895... | Line 968... | ||
895 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
968 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
896 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
969 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
897 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
970 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
898 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
971 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
Line 899... | Line 972... | ||
899 | 972 | ||
900 | DebugOut.Analog[22] = MittelIntegralRoll / 26; |
973 | //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
901 | //DebugOut.Analog[24] = GierGyroFehler; |
974 | //DebugOut.Analog[24] = GierGyroFehler; |
Line -... | Line 975... | ||
- | 975 | GierGyroFehler = 0; |
|
- | 976 | ||
- | 977 | //Salvo Ersatzkompass Ueberlauf korrigieren |
|
- | 978 | if (GyroKomp_Int >= ((long)360 * GIER_GRAD_FAKTOR)) GyroKomp_Int = GyroKomp_Int - (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
|
- | 979 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
|
Line 902... | Line 980... | ||
902 | GierGyroFehler = 0; |
980 | ROT_OFF; |
903 | 981 | // Salvo End |
|
904 | 982 | ||
905 | /*DebugOut.Analog[17] = IntegralAccNick / 26; |
983 | /*DebugOut.Analog[17] = IntegralAccNick / 26; |
Line 1013... | Line 1091... | ||
1013 | MittelIntegralNick2 = 0; |
1091 | MittelIntegralNick2 = 0; |
1014 | MittelIntegralRoll2 = 0; |
1092 | MittelIntegralRoll2 = 0; |
1015 | ZaehlMessungen = 0; |
1093 | ZaehlMessungen = 0; |
1016 | } |
1094 | } |
1017 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
1095 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
- | 1096 | // Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
|
- | 1097 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
|
- | 1098 | { |
|
- | 1099 | Kompass_Neuer_Wert = 0; |
|
- | 1100 | w = (abs(Mittelwert_AccNick)); |
|
- | 1101 | v = (abs(Mittelwert_AccRoll)); |
|
- | 1102 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
|
- | 1103 | { |
|
- | 1104 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
|
- | 1105 | { |
|
Line -... | Line 1106... | ||
- | 1106 | ||
- | 1107 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
|
- | 1108 | { |
|
- | 1109 | if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
|
- | 1110 | if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da |
|
- | 1111 | { |
|
- | 1112 | w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
|
- | 1113 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
|
- | 1114 | if (v <-180) v +=360; // Uberlaufkorrektur |
|
- | 1115 | if (v > 180) v -=360; // Uberlaufkorrektur |
|
- | 1116 | ||
- | 1117 | v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler |
|
- | 1118 | ||
- | 1119 | #define GIER_COMP_MAX 4 |
|
- | 1120 | if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
|
- | 1121 | if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
|
- | 1122 | if (abs(w) > 1) |
|
- | 1123 | { |
|
- | 1124 | GyroGier_Comp = 0; |
|
- | 1125 | gyrogier_kompass = KompassValue; // Kompasswert merken |
|
- | 1126 | AdNeutralGier -= v; |
|
- | 1127 | } |
|
- | 1128 | } |
|
- | 1129 | } |
|
- | 1130 | else |
|
- | 1131 | { |
|
- | 1132 | gyrogier_kompass = KompassValue; // Kompasswert merken |
|
- | 1133 | cnt_stickgier_zero = 0; |
|
- | 1134 | GyroGier_Comp = 0; |
|
- | 1135 | } |
|
- | 1136 | ||
- | 1137 | magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
|
- | 1138 | GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR; |
|
- | 1139 | ||
- | 1140 | w = KompassValue - GyroKomp_Int; |
|
- | 1141 | if ((w > 0) && (w < 180)) |
|
- | 1142 | { |
|
- | 1143 | ++GyroKomp_Int; |
|
- | 1144 | } |
|
- | 1145 | else if ((w > 0) && (w >= 180)) |
|
- | 1146 | { |
|
- | 1147 | --GyroKomp_Int; |
|
- | 1148 | } |
|
- | 1149 | else if ((w < 0) && (w >= -180)) |
|
- | 1150 | { |
|
- | 1151 | --GyroKomp_Int; |
|
- | 1152 | } |
|
- | 1153 | else if ((w < 0) && (w < -180)) |
|
- | 1154 | { |
|
- | 1155 | ++GyroKomp_Int; |
|
- | 1156 | } |
|
- | 1157 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L; |
|
- | 1158 | GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
|
- | 1159 | } |
|
- | 1160 | } |
|
- | 1161 | else //Kompassfehler |
|
- | 1162 | { |
|
- | 1163 | magkompass_ok = 0; |
|
- | 1164 | GyroGier_Comp = 0; |
|
- | 1165 | } |
|
- | 1166 | Kompass_Value_Old = KompassValue; |
|
- | 1167 | } |
|
- | 1168 | // Salvo End ************************* |
|
- | 1169 | ||
- | 1170 | // Salvo 6.10.2007 |
|
- | 1171 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
|
- | 1172 | //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
|
- | 1173 | if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
|
- | 1174 | && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
|
- | 1175 | { |
|
- | 1176 | if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv |
|
- | 1177 | { |
|
- | 1178 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
- | 1179 | else gps_cmd = GPS_CMD_REQ_HOME; |
|
- | 1180 | n = GPS_CRTL(gps_cmd); |
|
- | 1181 | } |
|
- | 1182 | else if ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv |
|
- | 1183 | { |
|
- | 1184 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
- | 1185 | else gps_cmd = GPS_CMD_REQ_HOLD; |
|
- | 1186 | n= GPS_CRTL(gps_cmd); |
|
- | 1187 | } |
|
- | 1188 | else // GPS komplett aus |
|
- | 1189 | { |
|
- | 1190 | if (gps_cmd != GPS_CMD_STOP) |
|
- | 1191 | { |
|
- | 1192 | gps_cmd = GPS_CMD_STOP; |
|
- | 1193 | n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
|
- | 1194 | } |
|
- | 1195 | } |
|
- | 1196 | } |
|
- | 1197 | else |
|
- | 1198 | { |
|
- | 1199 | if (gps_cmd != GPS_CMD_STOP) |
|
- | 1200 | { |
|
- | 1201 | gps_cmd = GPS_CMD_STOP; |
|
- | 1202 | n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
|
- | 1203 | } |
|
- | 1204 | } |
|
- | 1205 | if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12) LED_J16_OFF; //led im GPS Mode schnell blinken lassen |
|
1018 | 1206 | ||
1019 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1207 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1020 | // Gieren |
1208 | // Gieren |
1021 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1209 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1022 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1210 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1023 | if(abs(StickGier) > 15) // war 35 |
1211 | if(abs(StickGier) > 15) // war 35 |
Line 1038... | Line 1226... | ||
1038 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1226 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1039 | // Kompass |
1227 | // Kompass |
1040 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1228 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1041 | //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
1229 | //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
Line 1042... | Line -... | ||
1042 | - | ||
1043 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
- | |
1044 | { |
- | |
1045 | int w,v,r,fehler,korrektur; |
- | |
1046 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
- | |
1047 | v = abs(IntegralRoll /512); |
1230 | |
1048 | if(v > w) w = v; // grösste Neigung ermitteln |
- | |
1049 | korrektur = w / 8 + 1; |
- | |
1050 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1231 | // Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
1051 | if(NeueKompassRichtungMerken) |
- | |
1052 | { |
- | |
1053 | fehler = 0; |
- | |
1054 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
- | |
1055 | } |
- | |
1056 | if(!KompassSignalSchlecht && w < 25) |
1232 | if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
1057 | { |
- | |
1058 | GierGyroFehler += fehler; |
- | |
1059 | if(NeueKompassRichtungMerken) |
- | |
1060 | { |
- | |
1061 | beeptime = 200; |
1233 | { |
1062 | // KompassStartwert = KompassValue; |
- | |
1063 | KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1234 | KompassStartwert = KompassValue; |
1064 | NeueKompassRichtungMerken = 0; |
- | |
1065 | } |
1235 | NeueKompassRichtungMerken = 0; |
- | 1236 | } |
|
1066 | } |
1237 | // Salvo 13.9.2007 |
- | 1238 | w=0; |
|
1067 | ErsatzKompass += (fehler * 8) / korrektur; |
1239 | // Salvo End |
1068 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
1240 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
1069 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1241 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1070 | if(w >= 0) |
1242 | if(w >= 0) |
1071 | { |
- | |
1072 | if(!KompassSignalSchlecht) |
- | |
1073 | { |
1243 | { |
1074 | v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
- | |
1075 | r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
- | |
1076 | // r = KompassRichtung; |
1244 | // Salvo Kompasssteuerung ********************** |
1077 | v = (r * w) / v; // nach Kompass ausrichten |
- | |
1078 | w = 3 * Parameter_KompassWirkung; |
- | |
1079 | if(v > w) v = w; // Begrenzen |
- | |
1080 | else |
- | |
1081 | if(v < -w) v = -w; |
- | |
1082 | Mess_Integral_Gier += v; |
1245 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
1083 | } |
- | |
1084 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1246 | // Salvo End |
1085 | } |
- | |
- | 1247 | } |
|
1086 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1248 | |
1087 | } |
1249 | |
Line 1088... | Line 1250... | ||
1088 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1250 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1089 | 1251 | ||
1090 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1252 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1091 | // Debugwerte zuordnen |
1253 | // Debugwerte zuordnen |
1092 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1254 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1093 | if(!TimerWerteausgabe--) |
1255 | if(!TimerWerteausgabe--) |
- | 1256 | { |
|
- | 1257 | TimerWerteausgabe = 24; |
|
- | 1258 | // Salvo 13.12.2007 Beleuchtung steuern |
|
- | 1259 | if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
|
- | 1260 | else if (MotorenEin) LED_J16_ON; |
|
Line 1094... | Line 1261... | ||
1094 | { |
1261 | else LED_J16_OFF; |
1095 | TimerWerteausgabe = 24; |
1262 | // Salvo End |
1096 | 1263 | ||
1097 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
1264 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
1098 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
1265 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
1099 | DebugOut.Analog[2] = Mittelwert_AccNick; |
1266 | DebugOut.Analog[2] = Mittelwert_AccNick; |
1100 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
1267 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
- | 1268 | DebugOut.Analog[4] = MesswertGier; |
|
1101 | DebugOut.Analog[4] = MesswertGier; |
1269 | DebugOut.Analog[5] = HoehenWert; |
1102 | DebugOut.Analog[5] = HoehenWert; |
1270 | // DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
1103 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
1271 | |
1104 | DebugOut.Analog[8] = KompassValue; |
1272 | DebugOut.Analog[8] = KompassValue; |
- | 1273 | DebugOut.Analog[9] = UBat; |
|
1105 | DebugOut.Analog[9] = UBat; |
1274 | DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
1106 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1275 | // DebugOut.Analog[10] = SenderOkay; |
1107 | DebugOut.Analog[10] = SenderOkay; |
1276 | DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR; |
1108 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1277 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1109 | DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1278 | // DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1110 | DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
- | |
1111 | DebugOut.Analog[19] = WinkelOut.CalcState; |
- | |
1112 | DebugOut.Analog[20] = ServoValue; |
- | |
1113 | DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
- | |
Line 1114... | Line 1279... | ||
1114 | DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1279 | // DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
1115 | DebugOut.Analog[30] = GPS_Nick; |
1280 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
- | 1281 | // DebugOut.Analog[20] = ServoValue; |
|
- | 1282 | ||
- | 1283 | ||
- | 1284 | // DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
|
- | 1285 | // if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
|
- | 1286 | DebugOut.Analog[23] = debug_gp_0; |
|
- | 1287 | DebugOut.Analog[24] = debug_gp_1; |
|
- | 1288 | DebugOut.Analog[25] = debug_gp_2; |
|
- | 1289 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
|
- | 1290 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
|
- | 1291 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
|
Line 1116... | Line 1292... | ||
1116 | DebugOut.Analog[31] = GPS_Roll; |
1292 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
1117 | 1293 | ||
1118 | 1294 | DebugOut.Analog[30] = GPS_Nick; |
|
1119 | // DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
1295 | DebugOut.Analog[31] = GPS_Roll; |