From 81d51b1d1a7d5b7d81da7ece237bc980f3b6fd74 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 23 Mar 2019 13:48:07 +1100 Subject: [PATCH] AP_GPS: move print_latlon into sole caller --- .../examples/GPS_AUTO_test/GPS_AUTO_test.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) 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;