Rev 197 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 197 | Rev 209 | ||
---|---|---|---|
Line 37... | Line 37... | ||
37 | #define UBLOX_NAV_VELED 0x12 |
37 | #define UBLOX_NAV_VELED 0x12 |
38 | #define UBLOX_NAV_CLASS 0x01 |
38 | #define UBLOX_NAV_CLASS 0x01 |
39 | #define UBLOX_SYNCH1_CHAR 0xB5 |
39 | #define UBLOX_SYNCH1_CHAR 0xB5 |
40 | #define UBLOX_SYNCH2_CHAR 0x62 |
40 | #define UBLOX_SYNCH2_CHAR 0x62 |
Line 41... | Line 41... | ||
41 | 41 | ||
42 | signed int GPS_Nick = 0; |
42 | signed int GPS_Nick = 0; |
43 | signed int GPS_Roll = 0; |
43 | signed int GPS_Roll = 0; |
44 | short int ublox_msg_state = UBLOX_IDLE; |
44 | short int ublox_msg_state = UBLOX_IDLE; |
45 | static uint8_t chk_a =0; //Checksum |
45 | static uint8_t chk_a =0; //Checksum |
46 | static uint8_t chk_b =0; |
46 | static uint8_t chk_b =0; |
47 | short int gps_state; |
47 | short int gps_state; |
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; |
- | |
53 | signed int ; |
52 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
54 | static unsigned int rx_len; |
- | |
55 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
53 | static unsigned int rx_len; |
56 | static unsigned int ptr_payload_data_end; |
54 | static unsigned int ptr_payload_data_end; |
Line 57... | Line 55... | ||
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 |
58 | 56 | ||
Line 59... | Line 57... | ||
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 382... | Line 377... | ||
382 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
377 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
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 | { |
396 | dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
391 | dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
397 | dist = abs((dist *1000) / (long) sin_i(phi)); |
392 | dist = abs((dist *1000) / (long) sin_i(phi)); |
398 | } |
393 | } |
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 |