Rev 158 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 158 | Rev 159 | ||
---|---|---|---|
Line 47... | Line 47... | ||
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; |
|
52 | 53 | signed int ; |
|
53 | static unsigned int rx_len; |
54 | static unsigned int rx_len; |
54 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
55 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
55 | static unsigned int ptr_payload_data_end; |
56 | static unsigned int ptr_payload_data_end; |
Line 56... | Line 57... | ||
56 | 57 | ||
Line 81... | Line 82... | ||
81 | gps_act_position.status = 0; |
82 | gps_act_position.status = 0; |
82 | gps_rel_act_position.status = 0; |
83 | gps_rel_act_position.status = 0; |
83 | GPS_Nick = 0; |
84 | GPS_Nick = 0; |
84 | GPS_Roll = 0; |
85 | GPS_Roll = 0; |
85 | gps_updte_flag = 0; |
86 | gps_updte_flag = 0; |
- | 87 | gps_int_x = 0; |
|
- | 88 | gps_int_y = 0; |
|
- | 89 | ||
86 | } |
90 | } |
Line 87... | Line 91... | ||
87 | 91 | ||
88 | // Home Position sichern falls Daten verfuegbar sind. |
92 | // Home Position sichern falls Daten verfuegbar sind. |
89 | void GPS_Save_Home(void) |
93 | void GPS_Save_Home(void) |
Line 314... | Line 318... | ||
314 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
318 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
315 | cnt = 0; |
319 | cnt = 0; |
316 | GPS_Nick = 0; |
320 | GPS_Nick = 0; |
317 | GPS_Roll = 0; |
321 | GPS_Roll = 0; |
318 | gps_state = GPS_CRTL_IDLE; |
322 | gps_state = GPS_CRTL_IDLE; |
- | 323 | gps_int_x = 0; |
|
- | 324 | gps_int_y = 0; |
|
319 | return (GPS_STST_OK); |
325 | return (GPS_STST_OK); |
320 | break; |
326 | break; |
Line 321... | Line 327... | ||
321 | 327 | ||
322 | default: |
328 | default: |
Line 330... | Line 336... | ||
330 | cnt = 0; |
336 | cnt = 0; |
331 | return (GPS_STST_OK); |
337 | return (GPS_STST_OK); |
332 | break; |
338 | break; |
Line 333... | Line 339... | ||
333 | 339 | ||
334 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
340 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
335 | if (gps_updte_flag = 1) //nur wenn neue GPS Daten vorliegen |
341 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
336 | { |
342 | { |
337 | gps_updte_flag = 0; |
343 | gps_updte_flag = 0; |
338 | // ab hier wird geregelt |
- | |
339 | // Richtung zum Ziel ermitteln |
344 | // ab hier wird geregelt |
340 | signed int dist_north,dist_east; |
345 | signed int dist_north,dist_east; |
341 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
346 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
- | 347 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
|
- | 348 | ||
- | 349 | //PI Regler |
|
- | 350 | gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator |
|
- | 351 | gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16; |
|
- | 352 | if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096)) |
|
- | 353 | { |
|
- | 354 | gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen |
|
- | 355 | gps_int_y -= (dist_north * Parameter_UserParam1)/16; |
|
- | 356 | } |
|
- | 357 | gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu |
|
- | 358 | gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16; |
|
- | 359 | ||
342 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
360 | //Richtung bezogen auf Nordpol bestimmen |
- | 361 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
|
343 | GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north); |
362 | |
344 | //in Winkel 0..360 grad umrechnen |
363 | //in Winkel 0..360 grad umrechnen |
345 | if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
364 | if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
- | 365 | else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
|
- | 366 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
|
- | 367 | int n,m; |
|
- | 368 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
- | 369 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
|
- | 370 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
|
- | 371 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
|
346 | else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
372 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
347 | 373 | ||
348 | // Distanz zum Ziel ermitteln |
374 | // Distanz zum Ziel ermitteln |
349 | GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
375 | GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
350 | return (GPS_STST_OK); |
376 | return (GPS_STST_OK); |
351 | } |
377 | } |