From 079dd3c617c5d6a3e77f208dfbad2eb0d3254e7e Mon Sep 17 00:00:00 2001 From: "paulbmather@gmail.com" Date: Sun, 17 Oct 2010 04:07:46 +0000 Subject: [PATCH] Changes to accommodate blinking lights and loss of GPS commands. Tested with NMEA, uBlox, Sirf and MTK. Does not function correctly with rmack's changes to APM_Compass posted 10/17/2010 (I don't know why). git-svn-id: https://arducopter.googlecode.com/svn/trunk@669 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS_MTK.cpp | 26 ++++++++++++++------------ libraries/AP_GPS/AP_GPS_NMEA.cpp | 9 ++++++--- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 ++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 4 +++- libraries/AP_GPS/GPS.cpp | 17 +++++++++++++++++ libraries/AP_GPS/GPS.h | 8 ++++++++ 6 files changed, 50 insertions(+), 16 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 039b33e066..aa1eebc225 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -126,19 +126,21 @@ restart: void AP_GPS_MTK::_parse_gps(void) { - if (FIX_3D != _buffer.msg.fix_type) { - fix = false; - } else { + if (_buffer.msg.fix_type-1 >= 1){ fix = true; - latitude = _swapl(&_buffer.msg.latitude) * 10; - longitude = _swapl(&_buffer.msg.longitude) * 10; - altitude = _swapl(&_buffer.msg.altitude); - ground_speed = _swapl(&_buffer.msg.ground_speed); - ground_course = _swapl(&_buffer.msg.ground_course) / 10000; - num_sats = _buffer.msg.satellites; - - // XXX docs say this is UTC, but our clients expect msToW - time = _swapl(&_buffer.msg.utc_time); + } else { + fix = false; } + latitude = _swapl(&_buffer.msg.latitude) * 10; + longitude = _swapl(&_buffer.msg.longitude) * 10; + altitude = _swapl(&_buffer.msg.altitude); + ground_speed = _swapl(&_buffer.msg.ground_speed); + ground_course = _swapl(&_buffer.msg.ground_course) / 10000; + num_sats = _buffer.msg.satellites; + + // XXX docs say this is UTC, but our clients expect msToW + time = _swapl(&_buffer.msg.utc_time); + _setTime(); + valid_read = true; new_data = true; } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 9fec578933..4f9bf6f6c0 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -112,18 +112,18 @@ AP_GPS_NMEA::parse_nmea_gps(void) long aux_min; char *parseptr; - if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA if (buffer[bufferidx-4]=='*'){ // Check for the "*" character NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters if (GPS_checksum == NMEA_check){ // Checksum validation //Serial.println("buffer"); - new_data = 1; // New GPS Data + _setTime(); + valid_read = true; + new_data = true; // New GPS Data parseptr = strchr(buffer, ',')+1; //parseptr = strchr(parseptr, ',')+1; time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss parseptr = strchr(parseptr, ',')+1; - // aux_deg = parsedecimal(parseptr, 2); // degrees aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) @@ -166,6 +166,9 @@ AP_GPS_NMEA::parse_nmea_gps(void) if (buffer[bufferidx-4]=='*'){ // Check for the "*" character NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters if (GPS_checksum == NMEA_check){ // Checksum validation + _setTime(); + valid_read = true; + new_data = true; // New GPS Data parseptr = strchr(buffer, ',')+1; ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100 parseptr = strchr(parseptr, ',')+1; diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index a711bf94e7..20280e92c4 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -175,6 +175,8 @@ AP_GPS_SIRF::_parse_gps(void) if (ground_speed > 50) ground_course = _swapi(&_buffer.nav.ground_course); num_sats = _buffer.nav.satellites; + _setTime(); + valid_read = 1; break; } new_data = true; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 71cfc2cf8c..b49d01c376 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -184,5 +184,7 @@ AP_GPS_UBLOX::_parse_gps(void) ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 break; } - new_data = true; + _setTime(); + valid_read = 1; + new_data = 1; } diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index f84cb67306..011e40d77b 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -1,6 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #include "GPS.h" +#include "WProgram.h" #include void @@ -13,6 +14,22 @@ GPS::update(void) { } +void +GPS::_setTime(void){ + _lastTime = millis(); +} + +int +GPS::status(void){ + if (millis() - _lastTime >= 500){ + return 0; + } else if (fix == 0) { + return 1; + } else { + return 2; + } +} + void GPS::_error(const char *fmt, ...) { diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 1b93ec0195..8f48671b88 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -52,6 +52,9 @@ public: /// to false in order to avoid processing data they have /// already seen. bool new_data; + bool valid_read; + + int status(void); ///< 0 = Not Connected, 1 = Receiving Valid Messages but No Lock, 2 = Locked bool print_errors; ///< if true, errors will be printed to stderr @@ -66,6 +69,10 @@ protected: /// long _swapl(const void *bytes); + unsigned long _lastTime; ///< Timer for lost connection + + void _setTime(void); + /// perform an endian swap on an int /// /// @param bytes pointer to a buffer containing bytes representing an @@ -81,6 +88,7 @@ protected: /// @param fmt printf-like format string /// void _error(const char *msg, ...); + }; inline long