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
Unified 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)
void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, 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) float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{ {
time = _time; time = _time;
latitude = _latitude*1.0e7f; latitude = _latitude*1.0e7f;
longitude = _longitude*1.0e7f; longitude = _longitude*1.0e7f;
altitude = _altitude*1.0e2f; 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; ground_course = _ground_course*1.0e2f;
speed_3d = _speed_3d*1.0e2f; speed_3d = _speed_3d*1.0e2f;
num_sats = _num_sats; num_sats = _num_sats;
fix = true; fix = FIX_3D;
new_data = true; new_data = true;
_updated = true; _updated = true;
} }

9
libraries/AP_GPS/AP_GPS_MTK.cpp
Unescape View File

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

11
libraries/AP_GPS/AP_GPS_MTK19.cpp
Unescape View File

@ -136,8 +136,15 @@ restart:
goto restart; goto restart;
} }
fix = ((_buffer.msg.fix_type == FIX_3D) || // parse fix
(_buffer.msg.fix_type == FIX_3D_SBAS)); 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) { if (_mtk_revision == MTK_GPS_REVISION_V16) {
latitude = _buffer.msg.latitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise 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 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()
longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude * 10; // degrees*10e5 -> 10e7
ground_speed = _new_speed; ground_speed = _new_speed;
ground_course = _new_course; ground_course = _new_course;
fix = true; fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
break; break;
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude; altitude = _new_altitude;
@ -254,7 +254,7 @@ bool AP_GPS_NMEA::_term_complete()
longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude * 10; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count; num_sats = _new_satellite_count;
hdop = _new_hdop; hdop = _new_hdop;
fix = true; fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
break; break;
case _GPS_SENTENCE_GPVTG: case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed; ground_speed = _new_speed;
@ -268,7 +268,7 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about // Only these sentences give us information about
// fix status. // fix status.
fix = false; fix = GPS::FIX_NONE;
} }
} }
// we got a good message // 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)
switch(_msg_id) { switch(_msg_id) {
case MSG_GEONAV: case MSG_GEONAV:
time = _swapl(&_buffer.nav.time); time = _swapl(&_buffer.nav.time);
//fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); // parse fix type
fix = (0 == _buffer.nav.fix_invalid); 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); latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude); longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl); 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)
longitude = _buffer.posllh.longitude; longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude; latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10; altitude = _buffer.posllh.altitude_msl / 10;
fix = next_fix; fix = next_fix;
_new_position = true; _new_position = true;
break; break;
case MSG_STATUS: case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u", Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status, _buffer.status.fix_status,
_buffer.status.fix_type); _buffer.status.fix_type);
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
if (!next_fix) { next_fix = GPS::FIX_3D;
fix = false; }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; break;
case MSG_SOL: case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u", Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status, _buffer.solution.fix_status,
_buffer.solution.fix_type); _buffer.solution.fix_type);
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
if (!next_fix) { next_fix = GPS::FIX_3D;
fix = false; }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; num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP; hdop = _buffer.solution.position_DOP;

5
libraries/AP_GPS/AP_GPS_UBLOX.h
Unescape View File

@ -8,6 +8,7 @@
// License as published by the Free Software Foundation; either // License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version. // 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__ #ifndef __AP_GPS_UBLOX_H__
#define __AP_GPS_UBLOX_H__ #define __AP_GPS_UBLOX_H__
@ -39,7 +40,7 @@ public:
_payload_counter(0), _payload_counter(0),
_fix_count(0), _fix_count(0),
_disable_counter(0), _disable_counter(0),
next_fix(false) next_fix(GPS::FIX_NONE)
{} {}
// Methods // Methods
@ -207,7 +208,7 @@ private:
bool _parse_gps(); bool _parse_gps();
// used to update fix between status and position packets // 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_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_gps(void); void _configure_gps(void);

12
libraries/AP_GPS/GPS.cpp
Unescape View File

@ -21,7 +21,7 @@ GPS::GPS(void) :
// ensure all the inherited fields are zeroed // ensure all the inherited fields are zeroed
num_sats(0), num_sats(0),
new_data(false), new_data(false),
fix(false), fix(FIX_NONE),
valid_read(false), valid_read(false),
last_fix_time(0), last_fix_time(0),
_have_raw_velocity(false), _have_raw_velocity(false),
@ -56,7 +56,13 @@ GPS::update(void)
} }
} else { } else {
// we got a message, update our status correspondingly // 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; valid_read = true;
new_data = true; new_data = true;
@ -64,7 +70,7 @@ GPS::update(void)
// reset the idle timer // reset the idle timer
_idleTimer = tnow; _idleTimer = tnow;
if (_status == GPS_OK) { if (_status >= GPS_OK_FIX_2D) {
last_fix_time = _idleTimer; last_fix_time = _idleTimer;
_last_ground_speed_cm = ground_speed; _last_ground_speed_cm = ground_speed;

20
libraries/AP_GPS/GPS.h
Unescape View File

@ -34,7 +34,16 @@ public:
enum GPS_Status { enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock 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 // GPS navigation engine settings. Not all GPS receivers support
@ -106,8 +115,7 @@ public:
/// already seen. /// already seen.
bool new_data; bool new_data;
// Deprecated properties Fix_Status fix; ///< 0 if we have no fix, 2 for 2D fix, 3 for 3D fix
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) bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
// Debug support // Debug support
@ -127,13 +135,13 @@ public:
// components of velocity in 2D, in m/s // components of velocity in 2D, in m/s
float velocity_north(void) { float velocity_north(void) {
return _status == GPS_OK ? _velocity_north : 0; return _status >= GPS_OK_FIX_2D ? _velocity_north : 0;
} }
float velocity_east(void) { float velocity_east(void) {
return _status == GPS_OK ? _velocity_east : 0; return _status >= GPS_OK_FIX_2D ? _velocity_east : 0;
} }
float velocity_down(void) { 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 // last ground speed in m/s. This can be used when we have no GPS

Write Preview
Loading…
Cancel
Save