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 |