Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Adding support for the NMEA GPHDT sentence 

The NMEA GPHDT sentence can be used to determine the vehicles bearing
instead of a compass even when the vehicle is stationary.  This type
of GPS is normally very expensive and does the bearing using some sort
of phase ambituity algorithm.
This commit is contained in:
Grant Morphett 2017-05-28 14:11:31 +10:00 committed by Andrew Tridgell
parent fdcdbddecc
commit 64ed76326b
4 changed files with 68 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_GPS/AP_GPS.cpp
View File

@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: TYPE
// @DisplayName: GPS type
// @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
@ -488,7 +488,11 @@ void AP_GPS::detect_instance(uint8_t instance)
dstate->last_baud_change_ms = now;
if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
if (_type[instance] == GPS_TYPE_HEMI) {
send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING));
} else {
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
}
}
@ -540,7 +544,8 @@ void AP_GPS::detect_instance(uint8_t instance)
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
} else if (_type[instance] == GPS_TYPE_NMEA &&
} else if ((_type[instance] == GPS_TYPE_NMEA ||
_type[instance] == GPS_TYPE_HEMI) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}

30
libraries/AP_GPS/AP_GPS.h
View File

@ -84,7 +84,8 @@ public:
GPS_TYPE_GSOF = 11,
GPS_TYPE_ERB = 13,
GPS_TYPE_MAV = 14,
GPS_TYPE_NOVA = 15
GPS_TYPE_NOVA = 15,
GPS_TYPE_HEMI = 16, // hemisphere NMEA
};
/// GPS status codes
@ -130,6 +131,7 @@ public:
Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec
float ground_course; ///< ground course in degrees
float gps_yaw; ///< GPS derived yaw information, if available (degrees)
uint16_t hdop; ///< horizontal dilution of precision in cm
uint16_t vdop; ///< vertical dilution of precision in cm
uint8_t num_sats; ///< Number of visible satellites
@ -141,6 +143,7 @@ public:
bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available.
bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available.
bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available.
bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp()
@ -255,6 +258,23 @@ public:
return ground_course_cd(primary_instance);
}
// yaw in degrees if available
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
if (!have_gps_yaw(instance)) {
return false;
}
yaw_deg = state[instance].gps_yaw;
// None of the GPS backends can provide this yet, so we hard
// code a fixed value of 10 degrees, which seems like a
// reasonable guess. Once a backend can provide a proper
// estimate we can implement it
accuracy_deg = 10;
return true;
}
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {
return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg);
}
// number of locked satellites
uint8_t num_sats(uint8_t instance) const {
return state[instance].num_sats;
@ -329,6 +349,14 @@ public:
return have_vertical_velocity(primary_instance);
}
// return true if the GPS supports yaw
bool have_gps_yaw(uint8_t instance) const {
return state[instance].have_gps_yaw;
}
bool have_gps_yaw(void) const {
return have_gps_yaw(primary_instance);
}
// the expected lag (in seconds) in the position and velocity readings from the gps
// return true if the GPS hardware configuration is known or the lag parameter has been set manually
bool get_lag(uint8_t instance, float &lag_sec) const;

20
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -223,6 +223,12 @@ bool AP_GPS_NMEA::_have_new_message()
if (_last_VTG_ms != 0) {
_last_VTG_ms = 1;
}
if (now - _last_HDT_ms > 300) {
// we have lost GPS yaw
state.have_gps_yaw = false;
}
_last_GGA_ms = 1;
_last_RMC_ms = 1;
return true;
@ -293,6 +299,11 @@ bool AP_GPS_NMEA::_term_complete()
fill_3d_velocity();
// VTG has no fix indicator, can't change fix status
break;
case _GPS_SENTENCE_HDT:
_last_HDT_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true;
break;
}
} else {
switch (_sentence_type) {
@ -327,6 +338,10 @@ bool AP_GPS_NMEA::_term_complete()
_sentence_type = _GPS_SENTENCE_RMC;
} else if (strcmp(term_type, "GGA") == 0) {
_sentence_type = _GPS_SENTENCE_GGA;
} else if (strcmp(term_type, "HDT") == 0) {
_sentence_type = _GPS_SENTENCE_HDT;
// HDT doesn't have a data qualifier
_gps_data_good = true;
} else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good
@ -338,7 +353,7 @@ bool AP_GPS_NMEA::_term_complete()
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG
// 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
@ -400,6 +415,9 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal_100(_term) * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_HDT + 1: // Course (HDT)
_new_gps_yaw = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_VTG + 1: // Course (VTG)
_new_course = _parse_decimal_100(_term);

11
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -71,6 +71,7 @@ private:
_GPS_SENTENCE_RMC = 32,
_GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_OTHER = 0
};
@ -139,6 +140,7 @@ private:
int32_t _new_altitude; ///< altitude parsed from a term
int32_t _new_speed; ///< speed parsed from a term
int32_t _new_course; ///< course parsed from a term
float _new_gps_yaw; ///< yaw parsed from a term
uint16_t _new_hdop; ///< HDOP parsed from a term
uint8_t _new_satellite_count; ///< satellite count parsed from a term
uint8_t _new_quality_indicator; ///< GPS quality indicator parsed from a term
@ -146,6 +148,7 @@ private:
uint32_t _last_RMC_ms = 0;
uint32_t _last_GGA_ms = 0;
uint32_t _last_VTG_ms = 0;
uint32_t _last_HDT_ms = 0;
/// @name Init strings
/// In ::init, an attempt is made to configure the GPS
@ -159,3 +162,11 @@ private:
static const char _initialisation_blob[];
};
#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \
"$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \
"$JASC,GPGGA,5\r\n" /* GGA at 5Hz */ \
"$JASC,GPRMC,5\r\n" /* RMC at 5Hz */ \
"$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \
"$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \
"$JMODE,SBASR,YES\r\n" /* Enable SBAS */