mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
23ccff2ccd
commit
079dd3c617
|
@ -126,10 +126,11 @@ restart:
|
||||||
void
|
void
|
||||||
AP_GPS_MTK::_parse_gps(void)
|
AP_GPS_MTK::_parse_gps(void)
|
||||||
{
|
{
|
||||||
if (FIX_3D != _buffer.msg.fix_type) {
|
if (_buffer.msg.fix_type-1 >= 1){
|
||||||
fix = false;
|
|
||||||
} else {
|
|
||||||
fix = true;
|
fix = true;
|
||||||
|
} else {
|
||||||
|
fix = false;
|
||||||
|
}
|
||||||
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
||||||
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
||||||
altitude = _swapl(&_buffer.msg.altitude);
|
altitude = _swapl(&_buffer.msg.altitude);
|
||||||
|
@ -139,6 +140,7 @@ AP_GPS_MTK::_parse_gps(void)
|
||||||
|
|
||||||
// XXX docs say this is UTC, but our clients expect msToW
|
// XXX docs say this is UTC, but our clients expect msToW
|
||||||
time = _swapl(&_buffer.msg.utc_time);
|
time = _swapl(&_buffer.msg.utc_time);
|
||||||
}
|
_setTime();
|
||||||
|
valid_read = true;
|
||||||
new_data = true;
|
new_data = true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -112,18 +112,18 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
long aux_min;
|
long aux_min;
|
||||||
char *parseptr;
|
char *parseptr;
|
||||||
|
|
||||||
|
|
||||||
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
|
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
|
||||||
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
||||||
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
||||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||||
//Serial.println("buffer");
|
//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(buffer, ',')+1;
|
||||||
//parseptr = strchr(parseptr, ',')+1;
|
//parseptr = strchr(parseptr, ',')+1;
|
||||||
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
|
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
//
|
|
||||||
aux_deg = parsedecimal(parseptr, 2); // degrees
|
aux_deg = parsedecimal(parseptr, 2); // degrees
|
||||||
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
|
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)
|
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
|
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
||||||
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
||||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||||
|
_setTime();
|
||||||
|
valid_read = true;
|
||||||
|
new_data = true; // New GPS Data
|
||||||
parseptr = strchr(buffer, ',')+1;
|
parseptr = strchr(buffer, ',')+1;
|
||||||
ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100
|
ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
|
|
|
@ -175,6 +175,8 @@ AP_GPS_SIRF::_parse_gps(void)
|
||||||
if (ground_speed > 50)
|
if (ground_speed > 50)
|
||||||
ground_course = _swapi(&_buffer.nav.ground_course);
|
ground_course = _swapi(&_buffer.nav.ground_course);
|
||||||
num_sats = _buffer.nav.satellites;
|
num_sats = _buffer.nav.satellites;
|
||||||
|
_setTime();
|
||||||
|
valid_read = 1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
new_data = true;
|
new_data = true;
|
||||||
|
|
|
@ -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
|
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
new_data = true;
|
_setTime();
|
||||||
|
valid_read = 1;
|
||||||
|
new_data = 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,6 +1,7 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
#include "GPS.h"
|
#include "GPS.h"
|
||||||
|
#include "WProgram.h"
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
void
|
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
|
void
|
||||||
GPS::_error(const char *fmt, ...)
|
GPS::_error(const char *fmt, ...)
|
||||||
{
|
{
|
||||||
|
|
|
@ -52,6 +52,9 @@ public:
|
||||||
/// to false in order to avoid processing data they have
|
/// to false in order to avoid processing data they have
|
||||||
/// already seen.
|
/// already seen.
|
||||||
bool new_data;
|
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
|
bool print_errors; ///< if true, errors will be printed to stderr
|
||||||
|
|
||||||
|
@ -66,6 +69,10 @@ protected:
|
||||||
///
|
///
|
||||||
long _swapl(const void *bytes);
|
long _swapl(const void *bytes);
|
||||||
|
|
||||||
|
unsigned long _lastTime; ///< Timer for lost connection
|
||||||
|
|
||||||
|
void _setTime(void);
|
||||||
|
|
||||||
/// perform an endian swap on an int
|
/// perform an endian swap on an int
|
||||||
///
|
///
|
||||||
/// @param bytes pointer to a buffer containing bytes representing an
|
/// @param bytes pointer to a buffer containing bytes representing an
|
||||||
|
@ -81,6 +88,7 @@ protected:
|
||||||
/// @param fmt printf-like format string
|
/// @param fmt printf-like format string
|
||||||
///
|
///
|
||||||
void _error(const char *msg, ...);
|
void _error(const char *msg, ...);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
inline long
|
inline long
|
||||||
|
|
Loading…
Reference in New Issue