mirror of https://github.com/ArduPilot/ardupilot
Revert the MTK GPS driver to only claiming a fix when the GPS has a 3D solution, per discussion with Paul.
Minor tidying and documentation. git-svn-id: https://arducopter.googlecode.com/svn/trunk@683 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
35b754bbae
commit
2e5be5d86b
|
@ -130,11 +130,7 @@ restart:
|
|||
void
|
||||
AP_GPS_MTK::_parse_gps(void)
|
||||
{
|
||||
if (_buffer.msg.fix_type-1 >= 1){
|
||||
fix = true;
|
||||
} else {
|
||||
fix = false;
|
||||
}
|
||||
fix = (_buffer.msg.fix_type == FIX_3D);
|
||||
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
||||
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
||||
altitude = _swapl(&_buffer.msg.altitude);
|
||||
|
|
|
@ -4,13 +4,9 @@
|
|||
#include "WProgram.h"
|
||||
#include <stdio.h>
|
||||
|
||||
void
|
||||
GPS::_setTime(void){
|
||||
_lastTime = millis();
|
||||
}
|
||||
|
||||
int
|
||||
GPS::status(void){
|
||||
GPS::status(void)
|
||||
{
|
||||
if (millis() - _lastTime >= 500){
|
||||
return 0;
|
||||
} else if (fix == 0) {
|
||||
|
@ -20,6 +16,12 @@ GPS::status(void){
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
GPS::_setTime(void)
|
||||
{
|
||||
_lastTime = millis();
|
||||
}
|
||||
|
||||
void
|
||||
GPS::_error(const char *fmt, ...)
|
||||
{
|
||||
|
|
|
@ -31,6 +31,8 @@ public:
|
|||
/// This routine performs any one-off initialisation required to set the
|
||||
/// GPS up for use.
|
||||
///
|
||||
/// Must be implemented by the GPS driver.
|
||||
///
|
||||
/// @param s Stream connected to the GPS module. If NULL, assumed to
|
||||
/// have been set up at constructor time.
|
||||
///
|
||||
|
@ -40,8 +42,23 @@ public:
|
|||
///
|
||||
/// This routine must be called periodically to process incoming data.
|
||||
///
|
||||
/// Must be implemented by the GPS driver.
|
||||
///
|
||||
virtual void update(void) = 0;
|
||||
|
||||
/// Query GPS status
|
||||
///
|
||||
/// The 'valid message' status indicates that a recognised message was
|
||||
/// received from the GPS within the last 500ms.
|
||||
///
|
||||
/// @todo should probably return an enumeration here.
|
||||
///
|
||||
/// @return 0 No GPS connected/detected
|
||||
/// @return 1 Receiving valid GPS messages but no lock
|
||||
/// @return 2 Receiving valid messages and locked
|
||||
///
|
||||
int status(void);
|
||||
|
||||
// Properties
|
||||
long time; ///< GPS time in milliseconds from the start of the week
|
||||
long latitude; ///< latitude in degrees * 10,000,000
|
||||
|
@ -51,20 +68,26 @@ public:
|
|||
long ground_course; ///< ground course in 100ths of a degree
|
||||
long speed_3d; ///< 3D speed in cm/sec (not always available)
|
||||
uint8_t num_sats; ///< Number of visible satelites
|
||||
bool fix; ///< true if we have a position fix
|
||||
|
||||
/// Set to true when new data arrives. A client may set this
|
||||
/// 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
|
||||
// Deprecated properties
|
||||
bool fix; ///< true if we have a position fix (use ::status instead)
|
||||
bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
|
||||
|
||||
// Debug support
|
||||
bool print_errors; ///< if true, errors will be printed to stderr
|
||||
|
||||
protected:
|
||||
Stream *_port; ///< stream port the GPS is attached to
|
||||
unsigned long _lastTime; ///< Timer for lost connection
|
||||
|
||||
/// reset the last-message-received timer used by ::status
|
||||
///
|
||||
void _setTime(void);
|
||||
|
||||
/// perform an endian swap on a long
|
||||
///
|
||||
|
@ -74,10 +97,6 @@ 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
|
||||
|
|
Loading…
Reference in New Issue