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); |