GPS: add 2D fix type
This commit is contained in:
parent
f077f54e6a
commit
d7454bb09e
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user