aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GPS: add 2D fix type

Browse Source
This commit is contained in:
Randy Mackay 2013-03-25 16:24:14 +09:00 committed by rmackay9
parent f077f54e6a
commit d7454bb09e
9 changed files with 71 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS_HIL.cpp
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,
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;
}

9
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -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
View File

@ -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
View File

@ -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

10
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -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
View File

@ -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
View File

@ -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);

12
libraries/AP_GPS/GPS.cpp
View File

@ -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;

20
libraries/AP_GPS/GPS.h
View File

@ -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