Rev 242 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 242 | Rev 258 | ||
|---|---|---|---|
| Line 3... | Line 3... | ||
| 3 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
3 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
| 4 | either version 3 of the License, or (at your option) any later version. |
4 | either version 3 of the License, or (at your option) any later version. |
| 5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
| 6 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
6 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 7 | GNU General Public License and GNU Lesser General Public License for more details. |
7 | GNU General Public License and GNU Lesser General Public License for more details. |
| 8 | You should have received a copy of GNU General Public License ((License_GPL.txt) and |
8 | You should have received a copy of GNU General Public License (License_GPL.txt) and |
| 9 | GNU Lesser General Public License (License_LGPL.txt) along with this program. |
9 | GNU Lesser General Public License (License_LGPL.txt) along with this program. |
| 10 | If not, see <http://www.gnu.org/licenses/>. |
10 | If not, see <http://www.gnu.org/licenses/>. |
| Line 11... | Line 11... | ||
| 11 | 11 | ||
| 12 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
12 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
| 13 | */ |
13 | */ |
| 14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 15 | Peter Muehlenbrock |
15 | Peter Muehlenbrock |
| 16 | Auswertung der Daten vom GPS im ublox Format |
16 | Auswertung der Daten vom GPS im ublox Format |
| 17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
| 18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
| 19 | Stand 7.10.2007 |
19 | Stand 8.10.2007 |
| 20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 21 | */ |
21 | */ |
| 22 | #include "main.h" |
22 | #include "main.h" |
| Line 40... | Line 40... | ||
| 40 | #define UBLOX_NAV_VELED 0x12 |
40 | #define UBLOX_NAV_VELED 0x12 |
| 41 | #define UBLOX_NAV_CLASS 0x01 |
41 | #define UBLOX_NAV_CLASS 0x01 |
| 42 | #define UBLOX_SYNCH1_CHAR 0xB5 |
42 | #define UBLOX_SYNCH1_CHAR 0xB5 |
| 43 | #define UBLOX_SYNCH2_CHAR 0x62 |
43 | #define UBLOX_SYNCH2_CHAR 0x62 |
| Line 44... | Line 44... | ||
| 44 | 44 | ||
| 45 | signed int GPS_Nick = 0; |
45 | signed int GPS_Nick = 0; |
| 46 | signed int GPS_Roll = 0; |
46 | signed int GPS_Roll = 0; |
| 47 | short int ublox_msg_state = UBLOX_IDLE; |
47 | short int ublox_msg_state = UBLOX_IDLE; |
| 48 | static uint8_t chk_a =0; //Checksum |
48 | static uint8_t chk_a =0; //Checksum |
| 49 | static uint8_t chk_b =0; |
49 | static uint8_t chk_b =0; |
| 50 | short int gps_state,gps_sub_state; //Zustaende der Statemachine |
50 | short int gps_state,gps_sub_state; //Zustaende der Statemachine |
| 51 | short int gps_updte_flag; |
51 | short int gps_updte_flag; |
| 52 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
52 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
| 53 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
53 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
| 54 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
54 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
| 55 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
55 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
| 56 | static unsigned int rx_len; |
56 | static unsigned int rx_len; |
| 57 | static unsigned int ptr_payload_data_end; |
57 | static unsigned int ptr_payload_data_end; |
| 58 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
58 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
| 59 | signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
59 | signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
| 60 | signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
60 | static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
| 61 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
61 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
| 62 | static uint8_t *ptr_payload_data; |
62 | static uint8_t *ptr_payload_data; |
| 63 | static uint8_t *ptr_pac_status; |
63 | static uint8_t *ptr_pac_status; |
| Line 64... | Line 64... | ||
| 64 | long int dist_flown; |
64 | long int dist_flown; |
| Line 65... | Line 65... | ||
| 65 | 65 | ||
| 66 | short int Get_GPS_data(void); |
66 | short int Get_GPS_data(void); |
| Line 71... | Line 71... | ||
| 71 | 71 | ||
| 72 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
72 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
| 73 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
73 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
| 74 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
74 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
| 75 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
75 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
| Line 76... | Line 76... | ||
| 76 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
76 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
| 77 | 77 | ||
| 78 | // Initialisierung |
78 | // Initialisierung |
| 79 | void GPS_Neutral(void) |
79 | void GPS_Neutral(void) |
| Line 163... | Line 163... | ||
| 163 | actual_speed.status = 0; |
163 | actual_speed.status = 0; |
| 164 | } |
164 | } |
| 165 | return (n); |
165 | return (n); |
| 166 | } |
166 | } |
| Line 167... | Line -... | ||
| 167 | - | ||
| 168 | 167 | ||
| 169 | /* |
168 | /* |
| 170 | Daten vom GPS im ublox MSG Format auswerten |
169 | Daten vom GPS im ublox MSG Format auswerten |
| 171 | Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen |
170 | Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen |
| 172 | // Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein |
171 | // Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein |
| Line 331... | Line 330... | ||
| 331 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
330 | //Richtung zur Home Positionbezogen auf Nordpol bestimmen |
| 332 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
331 | hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
| 333 | // in Winkel 0...360 Grad umrechnen |
332 | // in Winkel 0...360 Grad umrechnen |
| 334 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
333 | if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
| 335 | else hdng_2home = (270 - hdng_2home); |
334 | else hdng_2home = (270 - hdng_2home); |
| 336 | dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
335 | dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
| 337 | gps_state = GPS_CRTL_HOME_ACTIVE; |
336 | gps_state = GPS_CRTL_HOME_ACTIVE; |
| 338 | return (GPS_STST_OK); |
337 | return (GPS_STST_OK); |
| 339 | } |
338 | } |
| 340 | else |
339 | else |
| 341 | { |
340 | { |
| Line 415... | Line 414... | ||
| 415 | int d1,d2,d3; |
414 | int d1,d2,d3; |
| 416 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
415 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
| 417 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
416 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| 418 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
417 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
| Line 419... | Line 418... | ||
| 419 | 418 | ||
| 420 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
419 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| 421 | { |
420 | { |
| 422 | hold_fast = 1; // Schnell Regeln |
421 | // hold_fast = 1; // Schnell Regeln |
| 423 | hold_reset_int = 1; // Integrator ausschalten |
422 | hold_reset_int = 1; // Integrator ausschalten |
| 424 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
423 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| 425 | { |
424 | { |
| 426 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
425 | if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| 427 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
426 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| 428 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
427 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
| 429 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
428 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
| 430 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
429 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| 431 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
430 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| 432 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
431 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| 433 | } |
432 | } |
| 434 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
433 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| 435 | { |
- | |
| 436 | /* if (gps_sub_state == GPS_HOME_FAST_IN_TOL) hold_reset_int = 1; //Integrator einmal am Beginn des normalen Regelns resetten |
- | |
| 437 | else hold_reset_int = 0; |
434 | { |
| 438 | */ if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
435 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
| 439 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
436 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
| 440 | } |
437 | } |
| 441 | } |
438 | } |
| 442 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
439 | else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
| - | 440 | { |
|
| 443 | { |
441 | hold_reset_int = 0; // Integrator einschalten |
| 444 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
442 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| - | 443 | { |
|
| - | 444 | if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch |
|
| - | 445 | else gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
|
| - | 446 | dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
|
| - | 447 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
|
| - | 448 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
|
| - | 449 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
|
| 445 | { |
450 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| - | 451 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
|
| - | 452 | } |
|
| - | 453 | else |
|
| 446 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
454 | { |
| - | 455 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; |
|
| 447 | if (d3 > GPS_G2T_DIST_HOLD) |
456 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
| - | 457 | } |
|
| - | 458 | } |
|
| - | 459 | else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
|
| 448 | { |
460 | { |
| - | 461 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
|
| - | 462 | { |
|
| 449 | if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
463 | gps_sub_state = GPS_HOME_IN_TOL; |
| 450 | dist_flown += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
464 | hold_fast = 0; // Wieder normal regeln |
| 451 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
465 | hold_reset_int = 0; // Integrator wieder aktivieren |
| 452 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
466 | if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
| 453 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
467 | else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
| 454 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
- | |
| 455 | } |
468 | if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
| - | 469 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
|
| 456 | else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
470 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
| 457 | { |
- | |
| 458 | hold_fast = 0; // Wieder normal regeln |
- | |
| 459 | hold_reset_int = 0; // Integrator wieder aktivieren |
- | |
| 460 | if (gps_rel_hold_position.utm_east > GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
- | |
| 461 | else if (gps_rel_hold_position.utm_east < -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
- | |
| 462 | if (gps_rel_hold_position.utm_north > GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
- | |
| 463 | else if (gps_rel_hold_position.utm_north < - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
- | |
| 464 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
- | |
| 465 | { |
471 | { |
| 466 | gps_rel_hold_position.utm_east = 0; |
472 | gps_rel_hold_position.utm_east = 0; |
| 467 | gps_rel_hold_position.utm_north = 0; |
473 | gps_rel_hold_position.utm_north = 0; |
| 468 | gps_sub_state = GPS_HOME_FINISHED; |
- | |
| 469 | } |
474 | gps_sub_state = GPS_HOME_FINISHED; |
| 470 | } |
475 | } |
| 471 | } |
- | |
| 472 | else |
- | |
| 473 | { |
- | |
| 474 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
- | |
| 475 | if (gps_g2t_act_v > 0) gps_g2t_act_v--; |
- | |
| 476 | } |
476 | } |
| 477 | } |
- | |
| 478 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
- | |
| 479 | return (GPS_STST_OK); |
477 | } |
| - | 478 | } |
|
| 480 | } |
479 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
| 481 | else return (GPS_STST_OK); |
480 | return (GPS_STST_OK); |
| 482 | } |
481 | } |
| 483 | else |
482 | else // Keine GPS Daten verfuegbar, deswegen Abbruch |
| 484 | { |
483 | { |
| 485 | gps_state = GPS_CRTL_IDLE; //Abbruch |
484 | gps_state = GPS_CRTL_IDLE; |
| 486 | return (GPS_STST_ERR); |
485 | return (GPS_STST_ERR); |
| 487 | } |
486 | } |
| Line 517... | Line 516... | ||
| 517 | int_north = 0; |
516 | int_north = 0; |
| 518 | } |
517 | } |
| Line 519... | Line 518... | ||
| 519 | 518 | ||
| 520 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
519 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
| 521 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
- | |
| 522 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
- | |
| 523 | #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
520 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
| 524 | signed long dist,int_east1,int_north1; |
521 | signed long dist,int_east1,int_north1; |
| 525 | int phi; |
522 | int phi; |
| 526 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
523 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
| Line -... | Line 524... | ||
| - | 524 | dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln |
|
| - | 525 | ||
| 527 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
526 | if (hold_fast == 0) // je Nach Modus andere Verstaerkungskurve fuer Differenzierer |
| 528 | 527 | { |
|
| - | 528 | diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10 |
|
| - | 529 | if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen |
|
| - | 530 | } |
|
| - | 531 | else |
|
| 529 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
532 | { |
| - | 533 | diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10 |
|
| Line 530... | Line 534... | ||
| 530 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
534 | if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen |
| 531 | // if (hold_fast > 0) diff_v = 10; //im schnellen Modus schwache Wirkung des Differenzierers |
535 | } |
| 532 | 536 | ||
| 533 | //I Werte begrenzen |
537 | //I Werte begrenzen |
| Line 538... | Line 542... | ||
| 538 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
542 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
| 539 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
543 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
| 540 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
544 | else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
| Line 541... | Line 545... | ||
| 541 | 545 | ||
| - | 546 | int diff_p; |
|
| 542 | int diff_p; |
547 | if (hold_fast == 0) diff_p = 1; |
| 543 | if (hold_fast > 0) diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
- | |
| Line 544... | Line 548... | ||
| 544 | else diff_p = 1; |
548 | else diff_p = 2; //im schnellen Modus Proportionalanteil staerken |
| 545 | 549 | ||
| 546 | //PID Regler Werte aufsummieren |
550 | //PID Regler Werte aufsummieren |