407,9 → 407,9 |
// if (dist > 200) dist = 200; |
// GPS_dist_2trgt = (int )dist; |
|
// Winkel und Distanz in Nick und Roll groessen umrechnen |
// Winkel und Distanz in Nick und Rollgroessen umrechnen |
GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
GPS_Nick = (int) -((dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
|
#define GPS_V 8 |
if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
419,9 → 419,12 |
|
//Kleine Werte verstaerken, Grosse abschwaechen |
long int nick,roll; |
roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
int n,r; |
r = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000; |
nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000; |
GPS_Roll = (int) (roll / GPS_V); |
nick = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
GPS_Nick = (int) (nick / GPS_V); |
|
if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
431,14 → 434,14 |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
} |
else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war |
/* else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt |
{ |
if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
return (GPS_STST_OK); |
} |
return (GPS_STST_OK); |
}*/ |
} |
else return (GPS_STST_OK); |
break; |