|
|
|
@ -17,13 +17,21 @@ AP_GPS_Auto GPS(&Serial1, &gps);
@@ -17,13 +17,21 @@ AP_GPS_Auto GPS(&Serial1, &gps);
|
|
|
|
|
#define T6 1000000 |
|
|
|
|
#define T7 10000000 |
|
|
|
|
|
|
|
|
|
// print_latlon - prints an latitude or longitude value held in an int32_t |
|
|
|
|
// print_latlon - prints an latitude or longitude value held in an int32_t |
|
|
|
|
// 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() |
|
|
|
|