WebSVN - FlightCtrl - Diff - Rev 628 and 629 - /branches/salvo_gps/Basis



If not, see <http://www.gnu.org/licenses/>.

 
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
*/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
von Peter Muehlenbrock alias Salvo
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Rückstuerz zur Basis Funktion

Stand 9.1.2008
 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

 
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
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
 
// Initialisierung
void GPS_Neutral(void)

                {
                        actual_status.status            = 0;
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
                        actual_pos.status                       = 0; //neue ublox Messages anfordern
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd;
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd;
                        gps_act_position.heading        = actual_speed.heading/100000;
                        actual_speed.status             = 0;
                        gps_act_position.status         = 1;
                        n                               = 0; //Daten gueltig
                }
                else
                {
                        gps_act_position.status = 0; //Keine gueltigen Daten
                        actual_speed.status     = 0;
                        actual_status.status    = 0;
                        actual_pos.status               = 0; //neue ublox Messages anfordern
                        n                                               = 2;
                }
        }      
        return (n);    
}

                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
                                        else
                                        {
                                                ptr_payload_data                = &actual_pos;
                                                ptr_payload_data_end    = &actual_pos.status;
                                                ublox_msg_state                 = UBLOX_LEN1;
                                        }
                                        break;

 
                                case UBLOX_NAV_STATUS:

 
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
                        {
                                cnt++;
                                if (cnt > 50) // erst nach Verzoegerung 
                                {
                                        // Erst mal initialisieren
                                        cnt             = 0;
                                        gps_tick                = 0;                                   
                                        hold_fast               = 0;
                                        hold_reset_int  = 0; // Integrator enablen
                                        int_east                = 0, int_north  = 0;

                                                return (GPS_STST_OK);                          
                                        }
                                        else
                                        {
                                                gps_rel_start_position.status   =       0;  //Keine Daten verfuegbar
                                                gps_state                       =       GPS_CRTL_IDLE;
                                                return(GPS_STST_ERR); // Keine Daten da
                                        }
                                }
                                else return(GPS_STST_PEND); // noch warten
                        }

                                        gps_tick++;
                                        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 - 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-3) gps_g2t_act_v += 4; //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
                                                {
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
//                                                      dist_flown++;                           //Auch ausserhalb der Toleranz langsam erhoehen
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
                                                }
                                                hold_reset_int                  = 0; // Integrator einsschalten  
                                                hold_fast                               = 1; // Regler fuer schnellen Flug
                                                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
                                        }
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen

                                                        dist_flown              +=      GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                        gps_sub_state   =       GPS_HOME_RMPDWN_IN_TOL;
                                                }
                                                else
                                                {
                                                        dist_flown++;   //Auch ausserhalb der Toleranz langsam erhoehen
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
                                                }                                      
                                                hold_reset_int          = 0; // Integrator einsschalten           
                                                hold_fast                       = 1; // Regler fuer schnellen Flug
                                                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
                                        }                                                      
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
                                        {
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
                                                {
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
                                                        hold_fast               = 0;    // Wieder normal regeln
                                                        hold_reset_int  = 0;    // Integrator einsschalten                
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
                                                        {
                                                                gps_rel_hold_position.utm_east  = 0;
                                                                gps_rel_hold_position.utm_north = 0;
                                                                gps_sub_state                   = GPS_HOME_FINISHED;
                                                        }
                                                }
                                                else gps_sub_state      = GPS_HOME_OUTOF_TOL;
                                        }                                      
                                }
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
                                return (GPS_STST_OK);                                  
                        }
                        else  // Keine GPS Daten verfuegbar, deswegen Abbruch
                        {
                                gps_state       = GPS_CRTL_IDLE;       
                                return (GPS_STST_ERR);
                        }
                        break;

                        {
                                // ab hier wird geregelt
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
                                int_east        += dist_east;
                                int_north       += dist_north;
                                speed_east      = (int) (-actual_speed.speed_e);
                                speed_north     = (int) (-actual_speed.speed_n);
                                gps_updte_flag  = 0;  // Neue Werte koennen vom GPS geholt werden

 
                                #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
                                {
                                        if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen
                                }
                                if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren 
                                {
                                        int_ovfl_cnt    -= 1;
                                        int_east        = (int_east*7)/8; // Wert reduzieren
                                        int_north       = (int_north*7)/8;                                     

                                }
 
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
                                {
                                        int_east        = 0;   
                                        int_north       = 0;                                   
                                }
                                if (hold_fast > 0)  //schneller Coming Home Modus Einfluss des D-Anteils verkleinern
                                {
                                        amplfy_speed_east  = (Parameter_UserParam3/GPS_USR_PAR_FKT);
                                        amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT);

 
                                #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
 
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung

                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;

                                if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V);
                                else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V);
                                n  = n/GPS_V;
                                n1 = n1/GPS_V;
                                //Kleine Werte verstaerken, Grosse abschwaechen
/*                              n               = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
                                n_l             = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
                                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;
*/                               

 
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
                                {
                                        GPS_Roll        = 0;

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 152...	Line 152...
152	{	152	{
153	actual_status.status = 0;	153	actual_status.status = 0;
154	gps_act_position.utm_east = actual_pos.utm_east/10;	154	gps_act_position.utm_east = actual_pos.utm_east/10;
155	gps_act_position.utm_north = actual_pos.utm_north/10;	155	gps_act_position.utm_north = actual_pos.utm_north/10;
156	gps_act_position.utm_alt = actual_pos.utm_alt/10;	156	gps_act_position.utm_alt = actual_pos.utm_alt/10;
157	actual_pos.status = 0; //neue ublox Messages anfordern	157	actual_pos.status = 0; //neue ublox Messages anfordern
158	gps_act_position.speed_gnd = actual_speed.speed_gnd;	158	gps_act_position.speed_gnd = actual_speed.speed_gnd;
159	gps_act_position.speed_gnd = actual_speed.speed_gnd;	159	gps_act_position.speed_gnd = actual_speed.speed_gnd;
160	gps_act_position.heading = actual_speed.heading/100000;	160	gps_act_position.heading = actual_speed.heading/100000;
161	actual_speed.status = 0;	161	actual_speed.status = 0;
162	gps_act_position.status = 1;	162	gps_act_position.status = 1;
163	n = 0; //Daten gueltig	163	n = 0; //Daten gueltig
164	}	164	}
165	else	165	else
166	{	166	{
167	gps_act_position.status = 0; //Keine gueltigen Daten	167	gps_act_position.status = 0; //Keine gueltigen Daten
168	actual_speed.status = 0;	168	actual_speed.status = 0;
169	actual_status.status = 0;	169	actual_status.status = 0;
170	actual_pos.status = 0; //neue ublox Messages anfordern	170	actual_pos.status = 0; //neue ublox Messages anfordern
171	n = 2;	171	n = 2;
172	}	172	}
173	}	173	}
174	return (n);	174	return (n);
175	}	175	}
Line 209...	Line 209...
209	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden	209	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
210	else	210	else
211	{	211	{
212	ptr_payload_data = &actual_pos;	212	ptr_payload_data = &actual_pos;
213	ptr_payload_data_end = &actual_pos.status;	213	ptr_payload_data_end = &actual_pos.status;
214	ublox_msg_state = UBLOX_LEN1;	214	ublox_msg_state = UBLOX_LEN1;
215	}	215	}
216	break;	216	break;
Line 217...	Line 217...
217		217
218	case UBLOX_NAV_STATUS:	218	case UBLOX_NAV_STATUS:
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;
323	hold_fast = 0;	323	hold_fast = 0;
324	hold_reset_int = 0; // Integrator enablen	324	hold_reset_int = 0; // Integrator enablen
325	int_east = 0, int_north = 0;	325	int_east = 0, int_north = 0;
Line 347...	Line 347...
347	return (GPS_STST_OK);	347	return (GPS_STST_OK);
348	}	348	}
349	else	349	else
350	{	350	{
351	gps_rel_start_position.status = 0; //Keine Daten verfuegbar	351	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
352	gps_state = GPS_CRTL_IDLE;	352	gps_state = GPS_CRTL_IDLE;
353	return(GPS_STST_ERR); // Keine Daten da	353	return(GPS_STST_ERR); // Keine Daten da
354	}	354	}
355	}	355	}
356	else return(GPS_STST_PEND); // noch warten	356	else return(GPS_STST_PEND); // noch warten
357	}	357	}
Line 424...	Line 424...
424	gps_tick++;	424	gps_tick++;
425	int d1,d2,d3;	425	int d1,d2,d3;
426	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );	426	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
427	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );	427	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
428	d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel	428	d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel
429	debug_gp_2 = dist_2home; // zum Debuggen	429	debug_gp_2 = dist_2home; // zum Debuggen
430	debug_gp_3 = dist_flown; // zum Debuggen	430	debug_gp_3 = dist_flown; // zum Debuggen
431	debug_gp_4 = hdng_2home; // zum Debuggen	431	debug_gp_4 = hdng_2home; // zum Debuggen
432	// debug_gp_5 = amplfy_speed_north; // zum Debuggen	432	// debug_gp_5 = amplfy_speed_north; // zum Debuggen
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
450	dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);	450	dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
451	dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);	451	dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
452	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt	452	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
453	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt	453	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
454	}	454	}
455	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen	455	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
Line 459...	Line 459...
459	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit	459	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
460	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;	460	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
461	}	461	}
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	}
474	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)	474	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
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))
486	{	486	{
487	gps_rel_hold_position.utm_east = 0;	487	gps_rel_hold_position.utm_east = 0;
488	gps_rel_hold_position.utm_north = 0;	488	gps_rel_hold_position.utm_north = 0;
489	gps_sub_state = GPS_HOME_FINISHED;	489	gps_sub_state = GPS_HOME_FINISHED;
490	}	490	}
491	}	491	}
492	else gps_sub_state = GPS_HOME_OUTOF_TOL;	492	else gps_sub_state = GPS_HOME_OUTOF_TOL;
493	}	493	}
494	}	494	}
495	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung	495	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
496	return (GPS_STST_OK);	496	return (GPS_STST_OK);
497	}	497	}
498	else // Keine GPS Daten verfuegbar, deswegen Abbruch	498	else // Keine GPS Daten verfuegbar, deswegen Abbruch
499	{	499	{
500	gps_state = GPS_CRTL_IDLE;	500	gps_state = GPS_CRTL_IDLE;
501	return (GPS_STST_ERR);	501	return (GPS_STST_ERR);
502	}	502	}
503	break;	503	break;
Line 508...	Line 508...
508	{	508	{
509	// ab hier wird geregelt	509	// ab hier wird geregelt
510	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;	510	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
511	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;	511	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
512	int_east += dist_east;	512	int_east += dist_east;
513	int_north += dist_north;	513	int_north += dist_north;
514	speed_east = (int) (-actual_speed.speed_e);	514	speed_east = (int) (-actual_speed.speed_e);
515	speed_north = (int) (-actual_speed.speed_n);	515	speed_north = (int) (-actual_speed.speed_n);
516	gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden	516	gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden
Line 517...	Line 517...
517		517
518	#define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow	518	#define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow
519	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX))	519	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX))
520	{	520	{
521	if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen	521	if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen
522	}	522	}
523	if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren	523	if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren
524	{	524	{
525	int_ovfl_cnt -= 1;	525	int_ovfl_cnt -= 1;
526	int_east = (int_east*7)/8; // Wert reduzieren	526	int_east = (int_east*7)/8; // Wert reduzieren
527	int_north = (int_north*7)/8;	527	int_north = (int_north*7)/8;
Line 528...	Line 528...
528	}	528	}
529		529
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);
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 610...	Line 609...
610	if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V);	609	if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V);
611	else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V);	610	else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V);
612	n = n/GPS_V;	611	n = n/GPS_V;
613	n1 = n1/GPS_V;	612	n1 = n1/GPS_V;
614	//Kleine Werte verstaerken, Grosse abschwaechen	613	//Kleine Werte verstaerken, Grosse abschwaechen
615	/* n = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));	614	/* n = sin_i((GPS_Roll90)/(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_Nick90)/(GPS_NICKROLL_MAX GPS_V));	617	n = sin_i((GPS_Nick90)/(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;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c - Rev 628 → 629