From d7454bb09e86006c263f048156505ea733218a8d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Mar 2013 16:24:14 +0900 Subject: [PATCH] GPS: add 2D fix type --- libraries/AP_GPS/AP_GPS_HIL.cpp | 4 ++-- libraries/AP_GPS/AP_GPS_MTK.cpp | 9 ++++++++- libraries/AP_GPS/AP_GPS_MTK19.cpp | 11 +++++++++-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_SIRF.cpp | 10 ++++++++-- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 22 +++++++++++++++------- libraries/AP_GPS/AP_GPS_UBLOX.h | 5 +++-- libraries/AP_GPS/GPS.cpp | 12 +++++++++--- libraries/AP_GPS/GPS.h | 20 ++++++++++++++------ 9 files changed, 71 insertions(+), 28 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 24887dfc43..6ac6a5316a 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -33,7 +33,7 @@ bool AP_GPS_HIL::read(void) void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { - time = _time; + time = _time; latitude = _latitude*1.0e7f; longitude = _longitude*1.0e7f; altitude = _altitude*1.0e2f; @@ -41,7 +41,7 @@ void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float ground_course = _ground_course*1.0e2f; speed_3d = _speed_3d*1.0e2f; num_sats = _num_sats; - fix = true; + fix = FIX_3D; new_data = true; _updated = true; } diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 7d2ad4e871..3df0d66d3b 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -139,7 +139,14 @@ restart: break; } - fix = _buffer.msg.fix_type == FIX_3D; + // set fix type + if (_buffer.msg.fix_type == FIX_3D) { + fix = GPS::FIX_3D; + }else if (_buffer.msg.fix_type == FIX_2D) { + fix = GPS::FIX_2D; + }else{ + fix = GPS::FIX_NONE; + } latitude = _swapl(&_buffer.msg.latitude) * 10; longitude = _swapl(&_buffer.msg.longitude) * 10; altitude = _swapl(&_buffer.msg.altitude); diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 70f92a087f..bb538d8c33 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -136,8 +136,15 @@ restart: goto restart; } - fix = ((_buffer.msg.fix_type == FIX_3D) || - (_buffer.msg.fix_type == FIX_3D_SBAS)); + // parse fix + if (_buffer.msg.fix_type == FIX_3D || _buffer.msg.fix_type == FIX_3D_SBAS) { + fix = GPS::FIX_3D; + }else if (_buffer.msg.fix_type == FIX_2D || _buffer.msg.fix_type == FIX_2D_SBAS) { + fix = GPS::FIX_2D; + }else{ + fix = GPS::FIX_NONE; + } + if (_mtk_revision == MTK_GPS_REVISION_V16) { latitude = _buffer.msg.latitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise longitude = _buffer.msg.longitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 7279c26960..6e2ac54956 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -245,7 +245,7 @@ bool AP_GPS_NMEA::_term_complete() longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 ground_speed = _new_speed; ground_course = _new_course; - fix = true; + fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix break; case _GPS_SENTENCE_GPGGA: altitude = _new_altitude; @@ -254,7 +254,7 @@ bool AP_GPS_NMEA::_term_complete() longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 num_sats = _new_satellite_count; hdop = _new_hdop; - fix = true; + fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix break; case _GPS_SENTENCE_GPVTG: ground_speed = _new_speed; @@ -268,7 +268,7 @@ bool AP_GPS_NMEA::_term_complete() case _GPS_SENTENCE_GPGGA: // Only these sentences give us information about // fix status. - fix = false; + fix = GPS::FIX_NONE; } } // we got a good message diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index e7654f8298..fe155b2509 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -171,8 +171,14 @@ AP_GPS_SIRF::_parse_gps(void) switch(_msg_id) { case MSG_GEONAV: time = _swapl(&_buffer.nav.time); - //fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); - fix = (0 == _buffer.nav.fix_invalid); + // parse fix type + if (_buffer.nav.fix_invalid) { + fix = GPS::FIX_NONE; + }else if (_buffer.nav.fix_type & FIX_MASK == FIX_3D) { + fix = GPS::FIX_3D; + }else{ + fix = GPS::FIX_2D; + } latitude = _swapl(&_buffer.nav.latitude); longitude = _swapl(&_buffer.nav.longitude); altitude = _swapl(&_buffer.nav.altitude_msl); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b01f896aec..4f2eeadfa7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -221,25 +221,33 @@ AP_GPS_UBLOX::_parse_gps(void) longitude = _buffer.posllh.longitude; latitude = _buffer.posllh.latitude; altitude = _buffer.posllh.altitude_msl / 10; - fix = next_fix; + fix = next_fix; _new_position = true; break; case MSG_STATUS: Debug("MSG_STATUS fix_status=%u fix_type=%u", _buffer.status.fix_status, _buffer.status.fix_type); - next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); - if (!next_fix) { - fix = false; + if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) { + next_fix = GPS::FIX_3D; + }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) { + next_fix = GPS::FIX_2D; + }else{ + next_fix = GPS::FIX_NONE; + fix = GPS::FIX_NONE; } break; case MSG_SOL: Debug("MSG_SOL fix_status=%u fix_type=%u", _buffer.solution.fix_status, _buffer.solution.fix_type); - next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); - if (!next_fix) { - fix = false; + if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) { + next_fix = GPS::FIX_3D; + }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) { + next_fix = GPS::FIX_2D; + }else{ + next_fix = GPS::FIX_NONE; + fix = GPS::FIX_NONE; } num_sats = _buffer.solution.satellites; hdop = _buffer.solution.position_DOP; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index bf6ab0d885..c8422abf71 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -8,6 +8,7 @@ // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // +// UBlox Lea6H protocol: http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf #ifndef __AP_GPS_UBLOX_H__ #define __AP_GPS_UBLOX_H__ @@ -39,7 +40,7 @@ public: _payload_counter(0), _fix_count(0), _disable_counter(0), - next_fix(false) + next_fix(GPS::FIX_NONE) {} // Methods @@ -207,7 +208,7 @@ private: bool _parse_gps(); // used to update fix between status and position packets - bool next_fix; + Fix_Status next_fix; void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); void _configure_gps(void); diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 8080947642..ea4a9d521b 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -21,7 +21,7 @@ GPS::GPS(void) : // ensure all the inherited fields are zeroed num_sats(0), new_data(false), - fix(false), + fix(FIX_NONE), valid_read(false), last_fix_time(0), _have_raw_velocity(false), @@ -56,7 +56,13 @@ GPS::update(void) } } else { // we got a message, update our status correspondingly - _status = fix ? GPS_OK : NO_FIX; + if (fix == FIX_3D) { + _status = GPS_OK_FIX_3D; + }else if (fix == FIX_2D) { + _status = GPS_OK_FIX_2D; + }else{ + _status = NO_FIX; + } valid_read = true; new_data = true; @@ -64,7 +70,7 @@ GPS::update(void) // reset the idle timer _idleTimer = tnow; - if (_status == GPS_OK) { + if (_status >= GPS_OK_FIX_2D) { last_fix_time = _idleTimer; _last_ground_speed_cm = ground_speed; diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index fe4a98389b..3f7fdc8731 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -34,7 +34,16 @@ public: enum GPS_Status { NO_GPS = 0, ///< No GPS connected/detected NO_FIX = 1, ///< Receiving valid GPS messages but no lock - GPS_OK = 2 ///< Receiving valid messages and locked + GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock + GPS_OK_FIX_3D = 3 ///< Receiving valid messages and 3D lock + }; + + /// Fix status codes + /// + enum Fix_Status { + FIX_NONE = 0, ///< No fix + FIX_2D = 2, ///< 2d fix + FIX_3D = 3, ///< 3d fix }; // GPS navigation engine settings. Not all GPS receivers support @@ -106,8 +115,7 @@ public: /// already seen. bool new_data; - // Deprecated properties - bool fix; ///< true if we have a position fix (use ::status instead) + Fix_Status fix; ///< 0 if we have no fix, 2 for 2D fix, 3 for 3D fix bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) // Debug support @@ -127,13 +135,13 @@ public: // components of velocity in 2D, in m/s float velocity_north(void) { - return _status == GPS_OK ? _velocity_north : 0; + return _status >= GPS_OK_FIX_2D ? _velocity_north : 0; } float velocity_east(void) { - return _status == GPS_OK ? _velocity_east : 0; + return _status >= GPS_OK_FIX_2D ? _velocity_east : 0; } float velocity_down(void) { - return _status == GPS_OK ? _velocity_down : 0; + return _status >= GPS_OK_FIX_2D ? _velocity_down : 0; } // last ground speed in m/s. This can be used when we have no GPS