diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp index 7d037a28ea..6e1426e4ce 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp @@ -60,6 +60,28 @@ void setup() gps.init(serial_manager); } + +/* + print a int32_t lat/long in decimal degrees + */ +void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) +{ + int32_t dec_portion, frac_portion; + int32_t abs_lat_or_lon = labs(lat_or_lon); + + // extract decimal portion (special handling of negative numbers to ensure we round towards zero) + dec_portion = abs_lat_or_lon / 10000000UL; + + // extract fractional portion + frac_portion = abs_lat_or_lon - dec_portion*10000000UL; + + // print output including the minus sign + if( lat_or_lon < 0 ) { + s->printf("-"); + } + s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion); +} + void loop() { static uint32_t last_msg_ms;