WebSVN - FlightCtrl - Diff - Rev 186 and 190



signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                            
signed int ;
static unsigned int rx_len;
unsigned int cnt0,cnt1,cnt2; //******Provisorisch
static unsigned int ptr_payload_data_end;
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt

 
static uint8_t *ptr_payload_data;

static uint8_t *ptr_pac_status;

        GPS_Nick                                        =       0;
        GPS_Roll                                        =       0;
        gps_updte_flag                          =       0;
        gps_int_x                                       =       0;
        gps_int_y                                       =       0;
        gps_alive_cnt                           =       0;

 

}
 

short int Get_Rel_Position(void)
{
        short int n = 0;
        n = Get_GPS_data();
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
        if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
        {
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten

                                                gps_reg_y               = 0;
                                                dist_east               = 0;
                                                dist_north              = 0;
                                                diff_east_f             = 0;
                                                diff_north_f    = 0;
                                                diff_east               = 0;
                                                diff_north              = 0;
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;

                                else return(GPS_STST_PEND); // noch warten
                        }
                        break;

 
                case GPS_CMD_STOP: // Lageregelung beenden
                        cnt                             =       0;
                        GPS_Nick                =       0;
                        GPS_Roll                =       0;
                        gps_int_x               =       0;
                        gps_int_y               =       0;
                        gps_state       =       GPS_CRTL_IDLE;
                        return (GPS_STST_OK);

                        break;

                        {
                                gps_updte_flag = 0;
                                // ab hier wird geregelt
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzier schon mal abziehen
                                diff_north      = -dist_north;
                                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;
                                diff_east       += dist_east;   // Differenz zur vorhergehenden East Position
                                diff_north      += dist_north;  // Differenz zur vorhergehenden North Position

                                n                               = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX));
                                d                               = (GPS_DIST_P_MAX * (long)n) /1000;    
                                dist_north_p    = (int) d;

 
                                //PID Regler Werte aufsummieren
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east_p   * Parameter_UserParam1)/4)+ ((diff_east_f  * Parameter_UserParam3)/2);  // I + P +D  Anteil X Achse

                                gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p  * Parameter_UserParam1)/4)+ ((diff_north_f * Parameter_UserParam3)/2);  // I + P +D  Anteil Y Achse
 

                                //Ziel-Richtung bezogen auf Nordpol bestimmen

                               
                                // Regelabweichung aus x,y zu Ziel in Distanz umrechnen 
                                if (abs(gps_reg_x) > abs(gps_reg_y) )
                                {
                                        dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
                                        dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
                                }
                                else
                                {
                                        dist = (long)gps_reg_y;
                                        dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
 
                                }
                                GPS_dist_2trgt  = (int) dist;
                                // Winkel und Distanz in Nick und Rollgroessen umrechnen
                                GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
                                GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
                                       
                                #define GPS_V 8 
                                if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
                                else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);

Rev 186	Rev 190
Line 52...	Line 52...
52	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;	52	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
53	signed int ;	53	signed int ;
54	static unsigned int rx_len;	54	static unsigned int rx_len;
55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch	55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
56	static unsigned int ptr_payload_data_end;	56	static unsigned int ptr_payload_data_end;
-		57	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
Line 57...	Line 58...
57		58
58	static uint8_t *ptr_payload_data;	59	static uint8_t *ptr_payload_data;
Line 59...	Line 60...
59	static uint8_t *ptr_pac_status;	60	static uint8_t *ptr_pac_status;
Line 83...	Line 84...
83	GPS_Nick = 0;	84	GPS_Nick = 0;
84	GPS_Roll = 0;	85	GPS_Roll = 0;
85	gps_updte_flag = 0;	86	gps_updte_flag = 0;
86	gps_int_x = 0;	87	gps_int_x = 0;
87	gps_int_y = 0;	88	gps_int_y = 0;
-		89	gps_alive_cnt = 0;
Line 88...	Line 90...
88		90
Line 89...	Line 91...
89	}	91	}
90		92
Line 108...	Line 110...
108	short int Get_Rel_Position(void)	110	short int Get_Rel_Position(void)
109	{	111	{
110	short int n = 0;	112	short int n = 0;
111	n = Get_GPS_data();	113	n = Get_GPS_data();
112	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind	114	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
-		115	if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert
113	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt	116	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
114	{	117	{
115	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);	118	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
116	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);	119	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
117	gps_rel_act_position.status = 1; // gueltige Positionsdaten	120	gps_rel_act_position.status = 1; // gueltige Positionsdaten
Line 315...	Line 318...
315	gps_reg_y = 0;	318	gps_reg_y = 0;
316	dist_east = 0;	319	dist_east = 0;
317	dist_north = 0;	320	dist_north = 0;
318	diff_east_f = 0;	321	diff_east_f = 0;
319	diff_north_f = 0;	322	diff_north_f = 0;
320	diff_east = 0;	323	diff_east = 0;
321	diff_north = 0;	324	diff_north = 0;
322	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;	325	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
323	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;	326	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
324	gps_rel_hold_position.status = 1; // gueltige Positionsdaten	327	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
325	gps_state = GPS_CRTL_HOLD_ACTIVE;	328	gps_state = GPS_CRTL_HOLD_ACTIVE;
Line 335...	Line 338...
335	else return(GPS_STST_PEND); // noch warten	338	else return(GPS_STST_PEND); // noch warten
336	}	339	}
337	break;	340	break;
Line 338...	Line 341...
338		341
339	case GPS_CMD_STOP: // Lageregelung beenden	342	case GPS_CMD_STOP: // Lageregelung beenden
340	cnt = 0;	343	cnt = 0;
341	GPS_Nick = 0;	344	GPS_Nick = 0;
342	GPS_Roll = 0;	345	GPS_Roll = 0;
343	gps_int_x = 0;	346	gps_int_x = 0;
344	gps_int_y = 0;	347	gps_int_y = 0;
345	gps_state = GPS_CRTL_IDLE;	348	gps_state = GPS_CRTL_IDLE;
346	return (GPS_STST_OK);	349	return (GPS_STST_OK);
Line 347...	Line 350...
347	break;	350	break;
Line 363...	Line 366...
363	{	366	{
364	gps_updte_flag = 0;	367	gps_updte_flag = 0;
365	// ab hier wird geregelt	368	// ab hier wird geregelt
366	diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen	369	diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen
367	diff_north = -dist_north;	370	diff_north = -dist_north;
368	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;	371	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
369	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;	372	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
370	int_east += dist_east;	373	int_east += dist_east;
371	int_north += dist_north;	374	int_north += dist_north;
372	diff_east += dist_east; // Differenz zur vorhergehenden East Position	375	diff_east += dist_east; // Differenz zur vorhergehenden East Position
373	diff_north += dist_north; // Differenz zur vorhergehenden North Position	376	diff_north += dist_north; // Differenz zur vorhergehenden North Position
Line 397...	Line 400...
397	n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX));	400	n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX));
398	d = (GPS_DIST_P_MAX * (long)n) /1000;	401	d = (GPS_DIST_P_MAX * (long)n) /1000;
399	dist_north_p = (int) d;	402	dist_north_p = (int) d;
Line 400...	Line 403...
400		403
401	//PID Regler Werte aufsummieren	404	//PID Regler Werte aufsummieren
402	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/6)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse	405	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/4)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse
Line 403...	Line 406...
403	gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/6)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse	406	gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/4)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse
404		407
Line 405...	Line 408...
405	//Ziel-Richtung bezogen auf Nordpol bestimmen	408	//Ziel-Richtung bezogen auf Nordpol bestimmen
Line 418...	Line 421...
418		421
419	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen	422	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
420	if (abs(gps_reg_x) > abs(gps_reg_y) )	423	if (abs(gps_reg_x) > abs(gps_reg_y) )
421	{	424	{
422	dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen	425	dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
423	dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));	426	dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
424	}	427	}
425	else	428	else
426	{	429	{
427	dist = (long)gps_reg_y;	430	dist = (long)gps_reg_y;
428	dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));	431	dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
429	}	-
-		432	}
430		433	GPS_dist_2trgt = (int) dist;
431	// Winkel und Distanz in Nick und Rollgroessen umrechnen	434	// Winkel und Distanz in Nick und Rollgroessen umrechnen
432	GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);	435	GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
433	GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);	436	GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
434		437
435	#define GPS_V 8	438	#define GPS_V 8
436	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);	439	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
437	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);	440	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
438	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);	441	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 540 - Rev 186 → 190