Rev 184 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 184 | Rev 185 | ||
---|---|---|---|
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); |
Line 419... | Line 419... | ||
419 | 419 | ||
420 | //Kleine Werte verstaerken, Grosse abschwaechen |
420 | //Kleine Werte verstaerken, Grosse abschwaechen |
- | 421 | long int nick,roll; |
|
421 | long int nick,roll; |
422 | int n,r; |
- | 423 | r = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
|
- | 424 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
|
- | 425 | roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000; |
|
422 | roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
426 | nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000; |
423 | GPS_Roll = (int) (roll / GPS_V); |
- | |
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); |
Line 425... | Line 428... | ||
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); |
441 | } |
444 | }*/ |
442 | } |
445 | } |
Line 443... | Line 446... | ||
443 | else return (GPS_STST_OK); |
446 | else return (GPS_STST_OK); |