WebSVN - FlightCtrl - Diff - Rev 197 and 209



short int       gps_updte_flag;
signed int      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
signed int      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
signed int      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                               
 
static unsigned int rx_len;
 
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;

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

 
// Home Position sichern falls Daten verfuegbar sind. 
void GPS_Save_Home(void)

        short int n = 1;

 
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
 
        {
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
                {
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
                        gps_act_position.utm_north      = actual_pos.utm_north/10;

Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
*/
void Get_Ublox_Msg(uint8_t rx)
{
 
        switch (ublox_msg_state)
        {

 
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen

                                {
                                        int_east        -= dist_east;
                                        int_north   -= dist_north;                                     
                                }
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
                                #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
                                #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
                                signed long dist;
                                int phi;
                                phi = arctan_i(abs(dist_north),abs(dist_east));
                                if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
                                {

                                {
                                        dist = (long)dist_north;
                                        dist = abs((dist *1000) / (long) cos_i(phi));
                                }
                                diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen

 
                                //PID Regler Werte aufsummieren
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse

Rev 197	Rev 209
Line 48...	Line 48...
48	short int gps_updte_flag;	48	short int gps_updte_flag;
49	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol	49	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
50	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters	50	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
51	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel	51	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
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 ;	-
54	static unsigned int rx_len;	53	static unsigned int rx_len;
55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch	-
56	static unsigned int ptr_payload_data_end;	54	static unsigned int ptr_payload_data_end;
57	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt	55	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
Line 58...	Line 56...
58		56
59	static uint8_t *ptr_payload_data;	57	static uint8_t *ptr_payload_data;
Line 85...	Line 83...
85	GPS_Roll = 0;	83	GPS_Roll = 0;
86	gps_updte_flag = 0;	84	gps_updte_flag = 0;
87	gps_int_x = 0;	85	gps_int_x = 0;
88	gps_int_y = 0;	86	gps_int_y = 0;
89	gps_alive_cnt = 0;	87	gps_alive_cnt = 0;
90		-
91	}	88	}
Line 92...	Line 89...
92		89
93	// Home Position sichern falls Daten verfuegbar sind.	90	// Home Position sichern falls Daten verfuegbar sind.
94	void GPS_Save_Home(void)	91	void GPS_Save_Home(void)
Line 136...	Line 133...
136	short int n = 1;	133	short int n = 1;
Line 137...	Line 134...
137		134
138	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist	135	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
139	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))	136	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
140	{	-
141	cnt1++; //**** noch Rausschmeissen	137	{
142	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind	138	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143	{	139	{
144	gps_act_position.utm_east = actual_pos.utm_east/10;	140	gps_act_position.utm_east = actual_pos.utm_east/10;
145	gps_act_position.utm_north = actual_pos.utm_north/10;	141	gps_act_position.utm_north = actual_pos.utm_north/10;
Line 168...	Line 164...
168	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen	164	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
169	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein	165	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
170	*/	166	*/
171	void Get_Ublox_Msg(uint8_t rx)	167	void Get_Ublox_Msg(uint8_t rx)
172	{	168	{
173		-
174	switch (ublox_msg_state)	169	switch (ublox_msg_state)
175	{	170	{
Line 176...	Line 171...
176		171
177	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen	172	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
Line 383...	Line 378...
383	{	378	{
384	int_east -= dist_east;	379	int_east -= dist_east;
385	int_north -= dist_north;	380	int_north -= dist_north;
386	}	381	}
387	//Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind	382	// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
388	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. bei 0 ist die Verstaerkung 1	383	// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
389	#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10	384	#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
390	#define GPS_DIFF_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm)	385	#define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
391	signed long dist;	386	signed long dist;
392	int phi;	387	int phi;
393	phi = arctan_i(abs(dist_north),abs(dist_east));	388	phi = arctan_i(abs(dist_north),abs(dist_east));
394	if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln	389	if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
395	{	390	{
Line 400...	Line 395...
400	{	395	{
401	dist = (long)dist_north;	396	dist = (long)dist_north;
402	dist = abs((dist *1000) / (long) cos_i(phi));	397	dist = abs((dist *1000) / (long) cos_i(phi));
403	}	398	}
404	diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10	399	diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
405	if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V;	400	if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
Line 406...	Line 401...
406		401
407	//PID Regler Werte aufsummieren	402	//PID Regler Werte aufsummieren
408	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse	403	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 354 - Rev 197 → 209