AP_GPS - updated MTK rate to 10Hz and fixed lat/lon printing inaccuracy in Arducopter logs and AP_TEST example sketch

ArduCopter/Log.pde

@@ -431,7 +431,14 @@ uint32_t check_hash;
 	return -1;
 }
 
-
+// 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);
+}
 
 // Write an GPS packet. Total length : 30 bytes
 static void Log_Write_GPS()
@@ -459,19 +466,21 @@ static void Log_Read_GPS()
 {
 	int32_t temp1 	= DataFlash.ReadLong();			// 1 time
 	int8_t temp2 	= DataFlash.ReadByte();			// 2 sats
-	float temp3 	= DataFlash.ReadLong() / t7;	// 3 lat
+	int32_t temp3 	= DataFlash.ReadLong();			// 3 lat
-	float temp4 	= DataFlash.ReadLong() / t7;	// 4 lon
+	int32_t temp4 	= DataFlash.ReadLong();			// 4 lon
 	float temp5 	= DataFlash.ReadLong() / 100.0;	// 5 gps alt
 	float temp6 	= DataFlash.ReadLong() / 100.0;	// 6 sensor alt
 	int16_t temp7 	= DataFlash.ReadInt();			// 7 ground speed
 	int32_t temp8 	= DataFlash.ReadLong();// 8 ground course
 
 							//  1   2    3      4     5      6      7    8
-	Serial.printf_P(PSTR("GPS, %ld, %d, %4.7f, %4.7f, %4.4f, %4.4f, %d, %ld\n"),
+	Serial.printf_P(PSTR("GPS, %ld, %d, "),
 							temp1,		// 1 time
-							temp2,		// 2 sats
+							temp2);		// 2 sats
-							temp3,		// 3 lat
+	print_latlon(&Serial, temp3);
-							temp4,		// 4 lon
+	Serial.print_P(PSTR(", "));
+	print_latlon(&Serial, temp4);
+	Serial.printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
 							temp5,		// 5 gps alt
 							temp6,		// 6 sensor alt
 							temp7,		// 7 ground speed

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -28,8 +28,8 @@ AP_GPS_MTK::init(void)
     // XXX should assume binary, let GPS_AUTO handle dynamic config?
     _port->print(MTK_SET_BINARY);
 
-    // set 4Hz update rate
+    // set 10Hz update rate
-    _port->print(MTK_OUTPUT_4HZ);
+    _port->print(MTK_OUTPUT_10HZ);
 
     // set initial epoch code
     _epoch = TIME_OF_DAY;

libraries/AP_GPS/AP_GPS_MTK16.cpp

@@ -30,8 +30,8 @@ AP_GPS_MTK16::init(void)
     // XXX should assume binary, let GPS_AUTO handle dynamic config?
     _port->print(MTK_SET_BINARY);
 
-    // set 4Hz update rate
+    // set 10Hz update rate
-    _port->print(MTK_OUTPUT_4HZ);
+    _port->print(MTK_OUTPUT_10HZ);
 
     // set initial epoch code
     _epoch = TIME_OF_DAY;

libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde

@@ -7,8 +7,8 @@
 */
 
 #include <FastSerial.h>
+#include <AP_Common.h>
 #include <AP_GPS.h>
-#include <stdio.h>
 
 FastSerialPort0(Serial);
 FastSerialPort1(Serial1);

libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde

@@ -16,6 +16,15 @@ 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 
+// 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);
+}
+
 void setup()
 {
     Serial.begin(115200);
@@ -31,9 +40,11 @@ void loop()
     gps->update();
     if (gps->new_data) {
         if (gps->fix) {
-            Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
+            Serial.print("\nLat: ");
-                          (float)gps->latitude / T7,
+            print_latlon(&Serial,gps->latitude);
-                          (float)gps->longitude / T7,
+            Serial.print(" Lon: ");
+            print_latlon(&Serial,gps->longitude);
+            Serial.printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
                           (float)gps->altitude / 100.0,
                           (float)gps->ground_speed / 100.0,
                           (int)gps->ground_course / 100,

libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde

@@ -7,8 +7,8 @@
 */
 
 #include <FastSerial.h>
+#include <AP_Common.h>
 #include <AP_GPS.h>
-#include <stdio.h>
 
 FastSerialPort0(Serial);
 FastSerialPort1(Serial1);

libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde

@@ -6,7 +6,6 @@
 #include <FastSerial.h>
 #include <AP_Common.h>
 #include <AP_GPS.h>
-#include <stdio.h>
 
 FastSerialPort0(Serial);
 FastSerialPort1(Serial1);