diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index f843307d9c..850f1ad7a4 100644 --- a/ArduCopter/Log.pde +++ b/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 - float temp4 = DataFlash.ReadLong() / t7; // 4 lon + int32_t temp3 = DataFlash.ReadLong(); // 3 lat + 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 - temp3, // 3 lat - temp4, // 4 lon + temp2); // 2 sats + print_latlon(&Serial, temp3); + 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 diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index a50088e43e..363eae53a0 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/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 - _port->print(MTK_OUTPUT_4HZ); + // set 10Hz update rate + _port->print(MTK_OUTPUT_10HZ); // set initial epoch code _epoch = TIME_OF_DAY; diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index 659f60f71b..9b4f9a95b8 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/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 - _port->print(MTK_OUTPUT_4HZ); + // set 10Hz update rate + _port->print(MTK_OUTPUT_10HZ); // set initial epoch code _epoch = TIME_OF_DAY; diff --git a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde index 0d2267de53..ad167a7697 100644 --- a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde +++ b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde @@ -7,8 +7,8 @@ */ #include +#include #include -#include FastSerialPort0(Serial); FastSerialPort1(Serial1); diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index c6de4f3bf4..e956799fc9 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/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", - (float)gps->latitude / T7, - (float)gps->longitude / T7, + Serial.print("\nLat: "); + print_latlon(&Serial,gps->latitude); + 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, diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde index afac202a4e..ab9a0b3721 100644 --- a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -7,8 +7,8 @@ */ #include +#include #include -#include FastSerialPort0(Serial); FastSerialPort1(Serial1); diff --git a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde index 9e0cadf912..95bf9c11bc 100644 --- a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -6,7 +6,6 @@ #include #include #include -#include FastSerialPort0(Serial); FastSerialPort1(Serial1);