Rev 184 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 184 | Rev 185 | ||
|---|---|---|---|
| Line 405... | Line 405... | ||
| 405 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
405 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 406 | } |
406 | } |
| 407 | // if (dist > 200) dist = 200; |
407 | // if (dist > 200) dist = 200; |
| 408 | // GPS_dist_2trgt = (int )dist; |
408 | // GPS_dist_2trgt = (int )dist; |
| Line 409... | Line 409... | ||
| 409 | 409 | ||
| 410 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
410 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| 411 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
411 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| Line 412... | Line 412... | ||
| 412 | GPS_Nick = (int) -((dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
412 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| 413 | 413 | ||
| 414 | #define GPS_V 8 |
414 | #define GPS_V 8 |
| 415 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
415 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
| 416 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
416 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
| Line 417... | Line 417... | ||
| 417 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
417 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
| 418 | else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
418 | else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
| - | 419 | ||
| 419 | 420 | //Kleine Werte verstaerken, Grosse abschwaechen |
|
| - | 421 | long int nick,roll; |
|
| - | 422 | int n,r; |
|
| - | 423 | r = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
|
| 420 | //Kleine Werte verstaerken, Grosse abschwaechen |
424 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 421 | long int nick,roll; |
- | |
| 422 | roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
425 | roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000; |
| Line 423... | Line 426... | ||
| 423 | GPS_Roll = (int) (roll / GPS_V); |
426 | nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000; |
| 424 | nick = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
427 | GPS_Roll = (int) (roll / GPS_V); |
| 425 | GPS_Nick = (int) (nick / GPS_V); |
428 | GPS_Nick = (int) (nick / GPS_V); |
| 426 | 429 | ||
| 427 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
430 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
| 428 | { |
431 | { |
| 429 | GPS_Roll = 0; |
432 | GPS_Roll = 0; |
| 430 | GPS_Nick = 0; |
433 | GPS_Nick = 0; |
| 431 | gps_state = GPS_CRTL_IDLE; |
434 | gps_state = GPS_CRTL_IDLE; |
| 432 | return (GPS_STST_ERR); |
435 | return (GPS_STST_ERR); |
| 433 | } |
436 | } |
| 434 | else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war |
437 | /* else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt |
| 435 | { |
438 | { |
| 436 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
439 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| 437 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
440 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| 438 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
441 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
| 439 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
442 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
| 440 | return (GPS_STST_OK); |
443 | return (GPS_STST_OK); |
| Line 441... | Line 444... | ||
| 441 | } |
444 | }*/ |