Rev 224 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 224 | Rev 225 | ||
---|---|---|---|
Line 12... | Line 12... | ||
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | Peter Muehlenbrock |
13 | Peter Muehlenbrock |
14 | Auswertung der Daten vom GPS im ublox Format |
14 | Auswertung der Daten vom GPS im ublox Format |
15 | Hold Modus mit PID Regler |
15 | Hold Modus mit PID Regler |
16 | Komm heim zu Papi Funktion |
16 | Komm heim zu Papi Funktion |
17 | Stand 5.10.2007 |
17 | Stand 6.10.2007 |
18 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
18 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | */ |
19 | */ |
20 | #include "main.h" |
20 | #include "main.h" |
21 | //#include "gps.h" |
21 | //#include "gps.h" |
Line 43... | Line 43... | ||
43 | signed int GPS_Nick = 0; |
43 | signed int GPS_Nick = 0; |
44 | signed int GPS_Roll = 0; |
44 | signed int GPS_Roll = 0; |
45 | short int ublox_msg_state = UBLOX_IDLE; |
45 | short int ublox_msg_state = UBLOX_IDLE; |
46 | static uint8_t chk_a =0; //Checksum |
46 | static uint8_t chk_a =0; //Checksum |
47 | static uint8_t chk_b =0; |
47 | static uint8_t chk_b =0; |
48 | short int gps_state,gps_sub_state; |
48 | short int gps_state,gps_sub_state; //Zustaende der Statemachine |
49 | short int gps_updte_flag; |
49 | short int gps_updte_flag; |
50 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
50 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
51 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
51 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
52 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
52 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
53 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
53 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
54 | static unsigned int rx_len; |
54 | static unsigned int rx_len; |
55 | static unsigned int ptr_payload_data_end; |
55 | static unsigned int ptr_payload_data_end; |
56 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
56 | unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
57 | signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
57 | signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
58 | signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
58 | signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
59 | - | ||
- | 59 | static short int hold_fast; //Flag fuer Hold Regler |
|
60 | static uint8_t *ptr_payload_data; |
60 | static uint8_t *ptr_payload_data; |
61 | static uint8_t *ptr_pac_status; |
61 | static uint8_t *ptr_pac_status; |
62 | - | ||
63 | long int dist_flown,dist_frm_start_east,dist_frm_start_north; |
62 | long int dist_flown; |
Line 64... | Line 63... | ||
64 | 63 | ||
Line 65... | Line 64... | ||
65 | short int Get_GPS_data(void); |
64 | short int Get_GPS_data(void); |
66 | 65 | ||
Line 292... | Line 291... | ||
292 | static signed int dist_north,dist_east; |
291 | static signed int dist_north,dist_east; |
293 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
292 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
294 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
293 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
295 | signed int n,diff_v; |
294 | signed int n,diff_v; |
296 | long signed int dev,n_l; |
295 | long signed int dev,n_l; |
297 | - | ||
- | 296 | signed int dist_frm_start_east,dist_frm_start_north; |
|
298 | switch (cmd) |
297 | switch (cmd) |
299 | { |
298 | { |
Line 300... | Line 299... | ||
300 | 299 | ||
301 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
300 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
Line 305... | Line 304... | ||
305 | if (cnt > 500) // erst nach Verzoegerung |
304 | if (cnt > 500) // erst nach Verzoegerung |
306 | { |
305 | { |
307 | // Erst mal initialisieren |
306 | // Erst mal initialisieren |
308 | cnt = 0; |
307 | cnt = 0; |
309 | gps_tick = 0; |
308 | gps_tick = 0; |
- | 309 | hold_fast = 0; |
|
310 | int_east = 0, int_north = 0; |
310 | int_east = 0, int_north = 0; |
311 | gps_reg_x = 0, gps_reg_y = 0; |
311 | gps_reg_x = 0, gps_reg_y = 0; |
312 | dist_east = 0, dist_north = 0; |
312 | dist_east = 0, dist_north = 0; |
313 | diff_east_f = 0, diff_north_f= 0; |
313 | diff_east_f = 0, diff_north_f= 0; |
314 | diff_east = 0, diff_north = 0; |
314 | diff_east = 0, diff_north = 0; |
Line 352... | Line 352... | ||
352 | { |
352 | { |
353 | cnt = 0; |
353 | cnt = 0; |
354 | // aktuelle positionsdaten abspeichern |
354 | // aktuelle positionsdaten abspeichern |
355 | if (gps_rel_act_position.status > 0) |
355 | if (gps_rel_act_position.status > 0) |
356 | { |
356 | { |
- | 357 | hold_fast = 0; |
|
357 | int_east = 0, int_north = 0; |
358 | int_east = 0, int_north = 0; |
358 | gps_reg_x = 0, gps_reg_y = 0; |
359 | gps_reg_x = 0, gps_reg_y = 0; |
359 | dist_east = 0, dist_north = 0; |
360 | dist_east = 0, dist_north = 0; |
360 | diff_east_f = 0, diff_north_f= 0; |
361 | diff_east_f = 0, diff_north_f= 0; |
361 | diff_east = 0, diff_north = 0; |
362 | diff_east = 0, diff_north = 0; |
Line 404... | Line 405... | ||
404 | { |
405 | { |
405 | if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist |
406 | if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist |
406 | { |
407 | { |
407 | gps_tick++; |
408 | gps_tick++; |
408 | int d1,d2,d3; |
409 | int d1,d2,d3; |
- | 410 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
|
- | 411 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
|
409 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
412 | d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
410 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
- | |
411 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
- | |
Line 412... | Line 413... | ||
412 | 413 | ||
413 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
414 | if (d3 > GPS_G2T_DIST_MAX_STOP) |
414 | { |
415 | { |
415 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
416 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
- | 417 | { |
|
416 | { |
418 | hold_fast = 1; // Schnell Regeln |
417 | dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit |
419 | dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit |
418 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
420 | dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
419 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
421 | dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
420 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
422 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
421 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
423 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
- | 424 | } |
|
422 | } |
425 | else hold_fast = 0; // Wieder normal regeln |
423 | } |
426 | } |
424 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
427 | else //Schon ziemlich nahe am Ziel, deswegen abbremsen |
- | 428 | { |
|
425 | { |
429 | hold_fast = 0; // Wieder normal regeln |
426 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
430 | if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
427 | { |
431 | { |
428 | if (d3 > 0) |
432 | if (d3 > GPS_G2T_DIST_HOLD) |
429 | { |
433 | { |
430 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
434 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
431 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
435 | dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000; |
- | 436 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
|
432 | dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000; |
437 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
433 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
- | |
434 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt |
438 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt |
435 | } |
439 | } |
436 | else //Ziel erreicht, Regelung beenden |
440 | else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
437 | { |
441 | { |
- | 442 | if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--; |
|
438 | gps_rel_hold_position.utm_east = 0; |
443 | else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++; |
439 | gps_rel_hold_position.utm_north = 0; |
444 | if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--; |
- | 445 | else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++; |
|
440 | gps_sub_state = GPS_HOME_FINISHED; |
446 | if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED; |
441 | } |
447 | } |
442 | } |
- | |
443 | 448 | } |
|
444 | } |
449 | } |
445 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
450 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
446 | return (GPS_STST_OK); |
451 | return (GPS_STST_OK); |
447 | } |
452 | } |
Line 476... | Line 481... | ||
476 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
481 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
477 | { |
482 | { |
478 | int_east -= dist_east; |
483 | int_east -= dist_east; |
479 | int_north -= dist_north; |
484 | int_north -= dist_north; |
480 | } |
485 | } |
- | 486 | if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten |
|
- | 487 | { |
|
- | 488 | int_east = 0; |
|
- | 489 | int_north = 0; |
|
- | 490 | } |
|
- | 491 | ||
481 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
492 | // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind |
482 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
493 | // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1 |
483 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
494 | #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10 |
484 | #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
495 | #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
485 | signed long dist; |
496 | signed long dist; |
Line 487... | Line 498... | ||
487 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
498 | phi = arctan_i(abs(dist_north),abs(dist_east)); |
488 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
499 | dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln |
Line 489... | Line 500... | ||
489 | 500 | ||
490 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
501 | diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10 |
- | 502 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
|
Line 491... | Line 503... | ||
491 | if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen |
503 | if (hold_fast > 0) diff_v = 10; // im schnellen Modus keine staerkere Wirkung des Differenzierers |
492 | 504 | ||
493 | //PID Regler Werte aufsummieren |
505 | //PID Regler Werte aufsummieren |