Rev 184 | 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 | }*/ |