Rev 632 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 632 | Rev 633 | ||
---|---|---|---|
Line 295... | Line 295... | ||
295 | } |
295 | } |
Line 296... | Line 296... | ||
296 | 296 | ||
297 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
297 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
298 | short int GPS_CRTL(short int cmd) |
298 | short int GPS_CRTL(short int cmd) |
299 | { |
299 | { |
300 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
300 | static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen |
301 | static signed int dist_north,dist_east; // Distanz zur Sollpoistion |
301 | static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition |
302 | signed int n,n1; |
302 | signed int n,n1; |
303 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
303 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
304 | long signed int dev; |
304 | long signed int dev; |
305 | signed int dist_frm_start_east,dist_frm_start_north; |
305 | signed int dist_frm_start_east,dist_frm_start_north; |
306 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
306 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
307 | static signed int int_east,int_north; //Integrierer |
307 | static signed int int_east,int_north; //Integrierer |
308 | long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
308 | long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
309 | signed long int_east1,int_north1; |
309 | signed long int_east1,int_north1; |
- | 310 | int dist_east,dist_north; |
|
- | 311 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
|
Line 310... | Line 312... | ||
310 | // signed long dist; |
312 | |
311 | 313 | ||
Line 312... | Line 314... | ||
312 | switch (cmd) |
314 | switch (cmd) |
Line 323... | Line 325... | ||
323 | gps_tick = 0; |
325 | gps_tick = 0; |
324 | hold_fast = 0; |
326 | hold_fast = 0; |
325 | hold_reset_int = 0; // Integrator enablen |
327 | hold_reset_int = 0; // Integrator enablen |
326 | int_east = 0, int_north = 0; |
328 | int_east = 0, int_north = 0; |
327 | gps_reg_x = 0, gps_reg_y = 0; |
329 | gps_reg_x = 0, gps_reg_y = 0; |
328 | dist_east = 0, dist_north = 0; |
330 | delta_east = 0, delta_north = 0; |
329 | dist_flown = 0; |
331 | dist_flown = 0; |
330 | gps_g2t_act_v = 0; |
332 | gps_g2t_act_v = 0; |
331 | gps_sub_state = GPS_CRTL_IDLE; |
333 | gps_sub_state = GPS_CRTL_IDLE; |
332 | // aktuelle positionsdaten abspeichern |
334 | // aktuelle positionsdaten abspeichern |
333 | if (gps_rel_act_position.status > 0) |
335 | if (gps_rel_act_position.status > 0) |
Line 371... | Line 373... | ||
371 | { |
373 | { |
372 | hold_fast = 0; |
374 | hold_fast = 0; |
373 | hold_reset_int = 0; // Integrator enablen |
375 | hold_reset_int = 0; // Integrator enablen |
374 | int_east = 0, int_north = 0; |
376 | int_east = 0, int_north = 0; |
375 | gps_reg_x = 0, gps_reg_y = 0; |
377 | gps_reg_x = 0, gps_reg_y = 0; |
376 | dist_east = 0, dist_north = 0; |
378 | delta_east = 0, delta_north = 0; |
377 | speed_east = 0; speed_north= 0; |
379 | speed_east = 0; speed_north= 0; |
378 | int_ovfl_cnt = 0; |
380 | int_ovfl_cnt = 0; |
379 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
381 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
380 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
382 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
381 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
383 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
Line 434... | Line 436... | ||
434 | 436 | ||
435 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
437 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
436 | { |
438 | { |
437 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
439 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
438 | { |
440 | { |
439 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit flott erhoehen |
441 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen |
440 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
442 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
441 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
443 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
442 | } |
444 | } |
443 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
445 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
Line 506... | Line 508... | ||
506 | 508 | ||
507 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
509 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
508 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
510 | if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
509 | { |
511 | { |
510 | // ab hier wird geregelt |
512 | // ab hier wird geregelt |
511 | dist_east = gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east; |
513 | delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east); |
512 | dist_north = gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north; |
514 | delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north); |
513 | int_east += dist_east; |
515 | int_east += (int)delta_east; |
514 | int_north += dist_north; |
516 | int_north += (int)delta_north; |
515 | speed_east = actual_speed.speed_e; |
517 | speed_east = actual_speed.speed_e; |
516 | speed_north = actual_speed.speed_n; |
518 | speed_north = actual_speed.speed_n; |
517 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
519 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
518 | debug_gp_2 = dist_east; // zum Debuggen |
520 | dist_east = (int)delta_east; //merken |
Line 519... | Line 521... | ||
519 | debug_gp_3 = dist_north; // zum Debuggen |
521 | dist_north = (int)delta_north; |
520 | 522 | ||
521 | 523 | ||
522 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
524 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
523 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
525 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
524 | { |
526 | { |
525 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
527 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
526 | } |
528 | } |
527 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
529 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
528 | { |
530 | { |
529 | int_ovfl_cnt -= 1; |
531 | int_ovfl_cnt -= 1; |
Line 530... | Line 532... | ||
530 | int_east = (int_east*7)/8; // Wert reduzieren |
532 | int_east = (int_east*7)/8; // Wert reduzieren |
531 | int_north = (int_north*7)/8; |
533 | int_north = (int_north*7)/8; |
Line 543... | Line 545... | ||
543 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
545 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10; |
544 | if (amplfy_speed_east > (DIFF_Y_F_MAX *10)) amplfy_speed_east = DIFF_Y_F_MAX *10; // Begrenzung |
546 | if (amplfy_speed_east > (DIFF_Y_F_MAX *10)) amplfy_speed_east = DIFF_Y_F_MAX *10; // Begrenzung |
545 | if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung |
547 | if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung |
546 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
548 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
547 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
549 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
- | 550 | speed_east = speed_east * abs(speed_east) /200; //quadrieren |
|
- | 551 | speed_north = speed_north * abs(speed_north) /200; //quadrieren |
|
- | 552 | diff_p = (Parameter_UserParam1 * GPS_PROP_FAST_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
|
548 | } |
553 | } |
549 | else //langsamer Holdmodus |
554 | else //langsamer Holdmodus |
550 | { |
555 | { |
551 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
556 | // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen |
552 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
557 | amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
553 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
558 | amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10; |
554 | if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
559 | if (amplfy_speed_east > (DIFF_Y_N_MAX *10)) amplfy_speed_east = DIFF_Y_N_MAX *10; // Begrenzung |
555 | if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
560 | if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung |
556 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
561 | amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
557 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
562 | amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
558 | speed_east = speed_east * abs(speed_east) /100; //quadrieren |
563 | speed_east = speed_east * abs(speed_east) /100; //quadrieren |
559 | speed_north = speed_north * abs(speed_north) /100; //quadrieren |
564 | speed_north = speed_north * abs(speed_north) /100; //quadrieren |
- | 565 | diff_p = (Parameter_UserParam1 * GPS_PROP_NRML_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
|
- | 566 | ||
560 | } |
567 | } |
561 | // amplfy_speed_east /= 10; |
- | |
562 | // amplfy_speed_north /= 10; |
- | |
563 | debug_gp_4 = (int)speed_east; // zum Debuggen |
568 | debug_gp_4 = (int)speed_east; // zum Debuggen |
564 | debug_gp_5 = (int)speed_north; // zum Debuggen |
569 | debug_gp_5 = (int)speed_north; // zum Debuggen |
Line 565... | Line -... | ||
565 | - | ||
566 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
- | |
567 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
- | |
568 | else diff_p = GPS_PROP_NRML_V; |
- | |
569 | 570 | ||
570 | //I Werte begrenzen |
571 | //I Werte begrenzen |
571 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
572 | #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen |
572 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
573 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
573 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
574 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
574 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
575 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
575 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
576 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
576 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
577 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
Line -... | Line 578... | ||
- | 578 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
|
- | 579 | ||
- | 580 | //P-Werte quadrieren |
|
- | 581 | delta_east = (abs(delta_east) * delta_east )/20; //quadrieren |
|
- | 582 | delta_north = (abs(delta_north) * delta_north)/20; //quadrieren |
|
- | 583 | debug_gp_2 = (int)delta_east; // zum Debuggen |
|
577 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
584 | debug_gp_3 = (int)delta_north; // zum Debuggen |
578 | 585 | ||
579 | //PID Regler Werte aufsummieren |
586 | //PID Regler Werte aufsummieren |
580 | gps_reg_x = -(int_east1 + (long)((dist_east * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east * (long)amplfy_speed_east )/(100L))); // I + P +D Anteil X Achse |
587 | gps_reg_x = -(int_east1 + ((delta_east * (long)diff_p)/(8*2))+ ((speed_east * (long)amplfy_speed_east )/(100L))); // I + P +D Anteil X Achse |
581 | gps_reg_y = -(int_north1 + (long)((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * (long)amplfy_speed_north )/(100L))); // I + P +D Anteil Y Achse |
588 | gps_reg_y = -(int_north1 + ((delta_north * (long)diff_p)/(8*2))+ ((speed_north * (long)amplfy_speed_north )/(100L))); // I + P +D Anteil Y Achse |
Line 582... | Line 589... | ||
582 | debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
589 | debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
583 | debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
590 | debug_gp_1 = (int)gps_reg_y; // zum Debuggen |