|
|
|
@ -21,9 +21,17 @@ AP_GPS_Auto GPS(&Serial1, &gps);
@@ -21,9 +21,17 @@ AP_GPS_Auto GPS(&Serial1, &gps);
|
|
|
|
|
// probably this should be moved to AP_Common |
|
|
|
|
void print_latlon(BetterStream *s, int32_t lat_or_lon) |
|
|
|
|
{ |
|
|
|
|
int32_t dec_portion = lat_or_lon / T7; |
|
|
|
|
int32_t frac_portion = labs(lat_or_lon - dec_portion*T7); |
|
|
|
|
s->printf("%ld.%07ld",dec_portion,frac_portion); |
|
|
|
|
int32_t dec_portion, frac_portion; |
|
|
|
|
|
|
|
|
|
// extract decimal portion (special handling of negative numbers to ensure we round towards zero) |
|
|
|
|
dec_portion = labs(lat_or_lon) / T7; |
|
|
|
|
if( lat_or_lon < 0 ){ |
|
|
|
|
dec_portion = -dec_portion; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// extract fractional portion |
|
|
|
|
frac_portion = labs(lat_or_lon - dec_portion*T7); |
|
|
|
|
s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void setup() |
|
|
|
|