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 fd071541a4..dd6fe2e323 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 @@ -1,57 +1,71 @@ -/* - Example of GPS NMEA library. - Code by Jordi Munoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) -*/ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Test for AP_GPS_NMEA +// #include +#include #include #include FastSerialPort0(Serial); FastSerialPort1(Serial1); -AP_GPS_NMEA gps(&Serial1); +AP_GPS_NMEA NMEA_gps(&Serial1); +GPS *gps = &NMEA_gps; + #define T6 1000000 #define T7 10000000 +const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble + 0x00, 0x18, // message length + 0x81, 0x02, // switch to NMEA + 0x01, 0x01, // GGA on with checksum + 0x00, 0x01, // GLL off + 0x00, 0x01, // GSA off + 0x00, 0x01, // GSV off + 0x01, 0x01, // RMC on with checksum + 0x01, 0x01, // VTG on with checksum + 0x00, 0x01, // MSS off + 0x00, 0x01, // EPE off + 0x00, 0x01, // ZPA off + 0x00, 0x00, // pad + 0x96, 0x00, // 38400 + 0x01, 0x25, // checksum TBD + 0xb0, 0xb3 }; // postamble + void setup() { Serial.begin(38400); Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; + + // try to coerce a SiRF unit that's been traumatized by + // AP_GPS_AUTO back into NMEA mode so that we can test + // it. + for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++) + Serial1.write(sirf_to_nmea[i]); Serial.println("GPS NMEA library test"); - gps.init(); // GPS Initialization - delay(1000); + gps->init(); } void loop() { - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have read the data + 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, + (float)gps->altitude / 100.0, + (float)gps->ground_speed / 100.0, + (int)gps->ground_course / 100, + gps->num_sats, + gps->time); + } else { + Serial.println("No fix"); } + gps->new_data = false; + } }