| 39,22 → 39,20 |
|---|
| #define UBLOX_SYNCH1_CHAR 0xB5 |
| #define UBLOX_SYNCH2_CHAR 0x62 |
| |
| signed int GPS_Nick = 0; |
| signed int GPS_Roll = 0; |
| short int ublox_msg_state = UBLOX_IDLE; |
| static uint8_t chk_a =0; //Checksum |
| static uint8_t chk_b =0; |
| short int gps_state; |
| 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; |
| signed int ; |
| signed int GPS_Nick = 0; |
| signed int GPS_Roll = 0; |
| short int ublox_msg_state = UBLOX_IDLE; |
| static uint8_t chk_a =0; //Checksum |
| static uint8_t chk_b =0; |
| short int gps_state; |
| 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; |
| 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 |
| unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
| |
| static uint8_t *ptr_payload_data; |
| static uint8_t *ptr_pac_status; |
| 87,7 → 85,6 |
|---|
| gps_int_x = 0; |
| gps_int_y = 0; |
| gps_alive_cnt = 0; |
| |
| } |
| |
| // Home Position sichern falls Daten verfuegbar sind. |
| 138,7 → 135,6 |
|---|
| 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)) |
| { |
| cnt1++; //**** noch Rausschmeissen |
| 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; |
| 170,7 → 166,6 |
|---|
| */ |
| void Get_Ublox_Msg(uint8_t rx) |
| { |
| |
| switch (ublox_msg_state) |
| { |
| |
| 384,13 → 379,13 |
|---|
| 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 1 |
| // 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 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm) |
| #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)); |
| phi = arctan_i(abs(dist_north),abs(dist_east)); |
| if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln |
| { |
| dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| 402,7 → 397,7 |
|---|
| 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; |
| 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 |