| 54,6 → 54,7 |
|---|
| 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; |
| 85,6 → 86,7 |
|---|
| gps_updte_flag = 0; |
| gps_int_x = 0; |
| gps_int_y = 0; |
| gps_alive_cnt = 0; |
| |
| } |
| |
| 110,6 → 112,7 |
|---|
| 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); |
| 317,7 → 320,7 |
|---|
| dist_north = 0; |
| diff_east_f = 0; |
| diff_north_f = 0; |
| diff_east = 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; |
| 337,11 → 340,11 |
|---|
| break; |
| |
| case GPS_CMD_STOP: // Lageregelung beenden |
| cnt = 0; |
| GPS_Nick = 0; |
| GPS_Roll = 0; |
| gps_int_x = 0; |
| gps_int_y = 0; |
| 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; |
| 365,7 → 368,7 |
|---|
| // 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_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; |
| 399,8 → 402,8 |
|---|
| dist_north_p = (int) d; |
| |
| //PID Regler Werte aufsummieren |
| 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 |
| 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 |
| 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 |
| GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| 420,18 → 423,18 |
|---|
| 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) cos_i(GPS_hdng_abs_2trgt)); |
| dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| } |
| else |
| { |
| dist = (long)gps_reg_y; |
| dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 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); |