| 16,7 → 16,7 |
|---|
| Auswertung der Daten vom GPS im ublox Format |
| Hold Modus mit PID Regler |
| Rückstuerz zur Basis Funktion |
| Stand 7.1.2008 |
| Stand 8.1.2008 |
| |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| 63,7 → 63,7 |
|---|
| static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
| static uint8_t *ptr_payload_data; |
| static uint8_t *ptr_pac_status; |
| static long int dist_flown; |
| static int dist_flown; |
| static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
| |
| |
| 77,7 → 77,7 |
|---|
| GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
| GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
| GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
| GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
| static GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
| |
| // Initialisierung |
| void GPS_Neutral(void) |
| 362,7 → 362,7 |
|---|
| if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
| { |
| cnt++; |
| if (cnt > 500) // erst nach Verzoegerung |
| if (cnt > 700) // erst nach Verzoegerung |
| { |
| cnt = 0; |
| // aktuelle positionsdaten abspeichern |
| 425,14 → 425,18 |
|---|
| int d1,d2,d3; |
| d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
| d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
| d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel |
| d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
| debug_gp_2 = dist_2home; // zum Debuggen |
| debug_gp_3 = dist_flown; // zum Debuggen |
| debug_gp_4 = hdng_2home; // zum Debuggen |
| // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
| |
| if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
| { |
| if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
| { |
| if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
| dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| if (gps_g2t_act_v < GPS_G2T_V_MAX-2) gps_g2t_act_v +=2; //Geschwindigkeit langsam erhoehen |
| dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
| gps_sub_state = GPS_HOME_FAST_IN_TOL; |
| } |
| else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
| 443,8 → 447,8 |
|---|
| } |
| hold_reset_int = 0; // Integrator einsschalten |
| hold_fast = 1; // Regler fuer schnellen Flug |
| dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
| dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
| dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
| dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
| gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| } |
| 462,8 → 466,8 |
|---|
| } |
| hold_reset_int = 0; // Integrator ausschalten |
| hold_fast = 1; // Wieder normal regeln |
| dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000); |
| dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000); |
| dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
| dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
| gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
| gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
| } |
| 515,8 → 519,6 |
|---|
| if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
| { |
| if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
| // int_east -= dist_east; auf alten Wert halten |
| // int_north -= dist_north; |
| } |
| if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
| { |
| 530,23 → 532,28 |
|---|
| int_east = 0; |
| int_north = 0; |
| } |
| if (hold_fast > 0) //schneller Coming Home Modus |
| { |
| amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| } |
| else //langsamer Holdmodus |
| { |
| // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen |
| amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| if (amplfy_speed_east > (DIFF_Y_MAX *10)) amplfy_speed_east = DIFF_Y_MAX *10; // Begrenzung |
| if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung |
| amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
| amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
| } |
| |
| // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen |
| amplfy_speed_east = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10; |
| if (amplfy_speed_east > (DIFF_Y_MAX *10)) amplfy_speed_east = DIFF_Y_MAX *10; // Begrenzung |
| if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung |
| amplfy_speed_east *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT); |
| amplfy_speed_east /= 10; //Faktor 10 wieder rausnehmen |
| amplfy_speed_north /= 10; //Faktor 10 wieder rausnehmen |
| |
| #define GPS_SPEED_SCALE 5 |
| speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
| speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
| debug_gp_2 = speed_east; // zum Debuggen |
| debug_gp_3 = speed_north; // zum Debuggen |
| debug_gp_4 = amplfy_speed_east; // zum Debuggen |
| debug_gp_5 = amplfy_speed_north; // zum Debuggen |
| |
| |
| int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
| 597,9 → 604,6 |
|---|
| // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| n = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); //Rollwert bestimmen |
| n1 = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); //Nickwert bestimmen |
| |
| // GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| // GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| |
| if (n > (GPS_NICKROLL_MAX * GPS_V)) n = (GPS_NICKROLL_MAX * GPS_V); |
| else if (n < -(GPS_NICKROLL_MAX * GPS_V)) n = -(GPS_NICKROLL_MAX * GPS_V); |
| 613,8 → 617,7 |
|---|
| GPS_Roll = (int) n_l; |
| n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| GPS_Nick = (int) n_l; |
| */ |
| GPS_Nick = (int) n_l;*/ |
| |
| if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
| { |