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 |