Rev 628 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 628 | Rev 629 | ||
---|---|---|---|
Line 10... | Line 10... | ||
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 | von Peter Muehlenbrock alias Salvo |
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 |
Line 19... | Line 19... | ||
19 | Stand 8.1.2008 |
19 | Stand 9.1.2008 |
20 | 20 | ||
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
22 | */ |
22 | */ |
Line 75... | Line 75... | ||
75 | 75 | ||
76 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
76 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
77 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
77 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
78 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
78 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
79 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
79 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
Line 80... | Line 80... | ||
80 | static GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
80 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
81 | 81 | ||
82 | // Initialisierung |
82 | // Initialisierung |
83 | void GPS_Neutral(void) |
83 | void GPS_Neutral(void) |
Line 313... | Line 313... | ||
313 | 313 | ||
314 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
314 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
315 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
315 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
316 | { |
316 | { |
317 | cnt++; |
317 | cnt++; |
318 | if (cnt > 200) // erst nach Verzoegerung |
318 | if (cnt > 50) // erst nach Verzoegerung |
319 | { |
319 | { |
320 | // Erst mal initialisieren |
320 | // Erst mal initialisieren |
321 | cnt = 0; |
321 | cnt = 0; |
322 | gps_tick = 0; |
322 | gps_tick = 0; |
Line 433... | Line 433... | ||
433 | 433 | ||
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
435 | { |
435 | { |
436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
437 | { |
437 | { |
438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-2) gps_g2t_act_v +=2; //Geschwindigkeit langsam erhoehen |
438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit langsam erhoehen |
439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
441 | } |
441 | } |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
443 | { |
443 | { |
444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
445 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
445 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
447 | } |
447 | } |
448 | hold_reset_int = 0; // Integrator einsschalten |
448 | hold_reset_int = 0; // Integrator einsschalten |
449 | hold_fast = 1; // Regler fuer schnellen Flug |
449 | hold_fast = 1; // Regler fuer schnellen Flug |
Line 462... | Line 462... | ||
462 | else |
462 | else |
463 | { |
463 | { |
464 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
464 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
465 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
465 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
466 | } |
466 | } |
467 | hold_reset_int = 0; // Integrator ausschalten |
467 | hold_reset_int = 0; // Integrator einsschalten |
468 | hold_fast = 1; // Wieder normal regeln |
468 | hold_fast = 1; // Regler fuer schnellen Flug |
469 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
469 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
470 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
470 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
471 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
471 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
472 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
472 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
473 | } |
473 | } |
Line 475... | Line 475... | ||
475 | { |
475 | { |
476 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
476 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
477 | { |
477 | { |
478 | gps_sub_state = GPS_HOME_IN_TOL; |
478 | gps_sub_state = GPS_HOME_IN_TOL; |
479 | hold_fast = 0; // Wieder normal regeln |
479 | hold_fast = 0; // Wieder normal regeln |
480 | hold_reset_int = 0; // Integrator wieder aktivieren |
480 | hold_reset_int = 0; // Integrator einsschalten |
481 | if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
481 | if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
482 | else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
482 | else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
483 | if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
483 | if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
Line 530... | Line 530... | ||
530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
531 | { |
531 | { |
532 | int_east = 0; |
532 | int_east = 0; |
533 | int_north = 0; |
533 | int_north = 0; |
534 | } |
534 | } |
535 | if (hold_fast > 0) //schneller Coming Home Modus |
535 | if (hold_fast > 0) //schneller Coming Home Modus Einfluss des D-Anteils verkleinern |
536 | { |
536 | { |
537 | amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
537 | amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
538 | amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
538 | amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
539 | } |
539 | } |
540 | else //langsamer Holdmodus |
540 | else //langsamer Holdmodus |
Line 553... | Line 553... | ||
553 | 553 | ||
554 | #define GPS_SPEED_SCALE 5 |
554 | #define GPS_SPEED_SCALE 5 |
555 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
555 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
Line 556... | Line -... | ||
556 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
- | |
557 | 556 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
|
558 | 557 | ||
559 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
558 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
Line 560... | Line 559... | ||
560 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
559 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
Line 615... | Line 614... | ||
615 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
614 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
616 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
615 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
617 | GPS_Roll = (int) n_l; |
616 | GPS_Roll = (int) n_l; |
618 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
617 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
619 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
618 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
620 | GPS_Nick = (int) n_l;*/ |
619 | GPS_Nick = (int) n_l; |
- | 620 | */ |
|
Line 621... | Line 621... | ||
621 | 621 | ||
622 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
622 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
623 | { |
623 | { |
624 | GPS_Roll = 0; |
624 | GPS_Roll = 0; |