zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

GPS: add 2D fix type

master
Randy Mackay 12 years ago committed by rmackay9
parent
f077f54e6a
commit
d7454bb09e
9 changed files with 71 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_GPS/AP_GPS_HIL.cpp
  2. 9
      libraries/AP_GPS/AP_GPS_MTK.cpp
  3. 11
      libraries/AP_GPS/AP_GPS_MTK19.cpp
  4. 6
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  5. 10
      libraries/AP_GPS/AP_GPS_SIRF.cpp
  6. 22
      libraries/AP_GPS/AP_GPS_UBLOX.cpp
  7. 5
      libraries/AP_GPS/AP_GPS_UBLOX.h
  8. 12
      libraries/AP_GPS/GPS.cpp
  9. 20
      libraries/AP_GPS/GPS.h

4
libraries/AP_GPS/AP_GPS_HIL.cpp
Unescape View File

@ -33,7 +33,7 @@ bool AP_GPS_HIL::read(void) @@ -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 @@ -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;
}

9
libraries/AP_GPS/AP_GPS_MTK.cpp
Unescape View File

@ -139,7 +139,14 @@ restart: @@ -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);

11
libraries/AP_GPS/AP_GPS_MTK19.cpp
Unescape View File

@ -136,8 +136,15 @@ restart: @@ -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

6
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

@ -245,7 +245,7 @@ bool AP_GPS_NMEA::_term_complete() @@ -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() @@ -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() @@ -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

10
libraries/AP_GPS/AP_GPS_SIRF.cpp
Unescape View File

@ -171,8 +171,14 @@ AP_GPS_SIRF::_parse_gps(void) @@ -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);

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
Unescape View File

@ -221,25 +221,33 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -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;

5
libraries/AP_GPS/AP_GPS_UBLOX.h
Unescape View File

@ -8,6 +8,7 @@ @@ -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: @@ -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: @@ -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);

12
libraries/AP_GPS/GPS.cpp
Unescape View File

@ -21,7 +21,7 @@ GPS::GPS(void) : @@ -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) @@ -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) @@ -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;

20
libraries/AP_GPS/GPS.h
Unescape View File

@ -34,7 +34,16 @@ public: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save