Rev 162 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 162 | Rev 167 | ||
|---|---|---|---|
| Line 281... | Line 281... | ||
| 281 | } |
281 | } |
| Line 282... | Line 282... | ||
| 282 | 282 | ||
| 283 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
283 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| 284 | short int GPS_CRTL(short int cmd) |
284 | short int GPS_CRTL(short int cmd) |
| 285 | { |
285 | { |
| 286 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
286 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| - | 287 | static int int_east,int_north; //Integrierer |
|
| - | 288 | static signed int dist_north,dist_east; |
|
| 287 | static int int_east,int_north; //Integrierer |
289 | int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| 288 | int n; |
290 | int n; |
| 289 | long int dist; |
291 | long int dist; |
| 290 | switch (cmd) |
292 | switch (cmd) |
| Line 347... | Line 349... | ||
| 347 | case GPS_CRTL_IDLE: |
349 | case GPS_CRTL_IDLE: |
| 348 | cnt = 0; |
350 | cnt = 0; |
| 349 | return (GPS_STST_OK); |
351 | return (GPS_STST_OK); |
| 350 | break; |
352 | break; |
| Line 351... | Line 353... | ||
| 351 | 353 | ||
| 352 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
354 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 353 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
355 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
| 354 | { |
356 | { |
| 355 | gps_updte_flag = 0; |
357 | gps_updte_flag = 0; |
| - | 358 | // ab hier wird geregelt |
|
| - | 359 | ||
| 356 | // ab hier wird geregelt |
360 | diff_east = -dist_east; //Alten Wert schon mal abziehen |
| 357 | signed int dist_north,dist_east; |
361 | diff_north = -dist_north; |
| 358 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
362 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| 359 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
363 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 360 | int_east += dist_east; |
364 | int_east += dist_east; |
| - | 365 | int_north += dist_north; |
|
| - | 366 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
|
| Line 361... | Line 367... | ||
| 361 | int_north += dist_north; |
367 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
| 362 | 368 | ||
| 363 | #define GPSINT_MAX 1000 |
369 | #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
| 364 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
370 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| 365 | { |
371 | { |
| 366 | int_east -= dist_east; |
372 | int_east -= dist_east; |
| Line 367... | Line 373... | ||
| 367 | int_north -= dist_north; |
373 | int_north -= dist_north; |
| 368 | } |
374 | } |
| 369 | 375 | ||
| 370 | #define DIST_MAX 200 //neu ab 19.9. 1900 Begrenzung |
376 | #define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung |
| 371 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
377 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
| Line 372... | Line 378... | ||
| 372 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
378 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
| 373 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
379 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
| 374 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
380 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
| Line 375... | Line 381... | ||
| 375 | 381 | ||
| 376 | //PI Regler |
382 | //PID Regler |
| Line 377... | Line 383... | ||
| 377 | gps_reg_x = ((int_east * Parameter_UserParam1)/16) + ((dist_east * Parameter_UserParam2)/8); // P+I Anteil |
383 | gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/2)+ ((diff_east * Parameter_UserParam3)/2); // |
| Line 385... | Line 391... | ||
| 385 | else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
391 | else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
| Line 386... | Line 392... | ||
| 386 | 392 | ||
| 387 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
393 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
| 388 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
394 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
| 389 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
395 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
| 390 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
396 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
| 391 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
397 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
| Line 392... | Line 398... | ||
| 392 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
398 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |