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

AP_GPS: support $PHD message for AllyStay NMEA GPS

Browse Source 
this adds vertical velocity support
This commit is contained in:
Andrew Tridgell 2020-11-12 10:04:14 +11:00
parent 211cd5255d
commit 862ae59e28
5 changed files with 155 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_GPS/AP_GPS.cpp
View File

@ -76,7 +76,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,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP
// @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,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
@ -85,7 +85,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,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP
// @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,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
@ -676,7 +676,8 @@ void AP_GPS::detect_instance(uint8_t instance)
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 ||
_type[instance] == GPS_TYPE_HEMI) &&
_type[instance] == GPS_TYPE_HEMI ||
_type[instance] == GPS_TYPE_ALLYSTAR) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}

1
libraries/AP_GPS/AP_GPS.h
View File

@ -114,6 +114,7 @@ public:
GPS_TYPE_UBLOX_RTK_BASE = 17,
GPS_TYPE_UBLOX_RTK_ROVER = 18,
GPS_TYPE_MSP = 19,
GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
};
/// GPS status codes

116
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -33,6 +33,7 @@
#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include "AP_GPS_NMEA.h"
@ -74,6 +75,41 @@ bool AP_GPS_NMEA::read(void)
return parsed;
}
/*
formatted print of NMEA message to the port, with checksum appended
*/
bool AP_GPS_NMEA::nmea_printf(const char *fmt, ...) const
{
char *s = nullptr;
char trailer[6];
va_list ap;
va_start(ap, fmt);
int ret = vasprintf(&s, fmt, ap);
va_end(ap);
if (ret == -1 || s == nullptr) {
// allocation failed
return false;
}
// calculate the checksum
uint8_t cs = 0;
const uint8_t *b = (const uint8_t *)s+1;
while (*b) {
cs ^= *b++;
}
uint32_t len = strlen(s);
snprintf(trailer, sizeof(trailer), "*%02X\r\n", (unsigned)cs);
if (port->txspace() < len + 5) {
free(s);
return false;
}
port->write((const uint8_t*)s, len);
port->write((const uint8_t*)trailer, 5);
free(s);
return true;
}
bool AP_GPS_NMEA::_decode(char c)
{
bool valid_sentence = false;
@ -210,6 +246,25 @@ bool AP_GPS_NMEA::_have_new_message()
now - _last_VTG_ms > 150) {
return false;
}
/*
if we have seen the $PHD messages then wait for them again. This
is important as the have_vertical_velocity field will be
overwritten by fill_3d_velocity()
*/
if (_last_PHD_12_ms != 0 &&
now - _last_PHD_12_ms > 150 &&
now - _last_PHD_12_ms < 1000) {
// waiting on PHD_12
return false;
}
if (_last_PHD_26_ms != 0 &&
now - _last_PHD_26_ms > 150 &&
now - _last_PHD_26_ms < 1000) {
// waiting on PHD_26
return false;
}
// prevent these messages being used again
if (_last_VTG_ms != 0) {
_last_VTG_ms = 1;
@ -220,6 +275,17 @@ bool AP_GPS_NMEA::_have_new_message()
state.have_gps_yaw = false;
}
// special case for fixing low output rate of ALLYSTAR GPS modules
const int32_t dt_ms = now - _last_fix_ms;
if (labs(dt_ms - gps._rate_ms[state.instance]) > 50 &&
get_type() == AP_GPS::GPS_TYPE_ALLYSTAR) {
nmea_printf("$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0",
1000U/gps._rate_ms[state.instance],
gps._rate_ms[state.instance]);
}
_last_fix_ms = now;
_last_GGA_ms = 1;
_last_RMC_ms = 1;
return true;
@ -253,7 +319,10 @@ bool AP_GPS_NMEA::_term_complete()
make_gps_time(_new_date, _new_time * 10);
set_uart_timestamp(_sentence_length);
state.last_gps_time_ms = now;
fill_3d_velocity();
if (_last_PHD_12_ms == 0 ||
now - _last_PHD_12_ms > 1000) {
fill_3d_velocity();
}
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
@ -293,7 +362,10 @@ bool AP_GPS_NMEA::_term_complete()
_last_VTG_ms = now;
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
fill_3d_velocity();
if (_last_PHD_12_ms == 0 ||
now - _last_PHD_12_ms > 1000) {
fill_3d_velocity();
}
// VTG has no fix indicator, can't change fix status
break;
case _GPS_SENTENCE_HDT:
@ -306,6 +378,22 @@ bool AP_GPS_NMEA::_term_complete()
// HDT sentence.
state.gps_yaw_configured = true;
break;
case _GPS_SENTENCE_PHD:
if (_phd.msg_id == 12) {
state.velocity.x = _phd.fields[0] * 0.01;
state.velocity.y = _phd.fields[1] * 0.01;
state.velocity.z = _phd.fields[2] * 0.01;
state.have_vertical_velocity = true;
_last_PHD_12_ms = now;
} else if (_phd.msg_id == 26) {
state.horizontal_accuracy = MAX(_phd.fields[0],_phd.fields[1]) * 0.001;
state.have_horizontal_accuracy = true;
state.vertical_accuracy = _phd.fields[2] * 0.001;
state.have_vertical_accuracy = true;
state.speed_accuracy = MAX(_phd.fields[3],_phd.fields[4]) * 0.001;
state.have_speed_accuracy = true;
_last_PHD_26_ms = now;
}
}
} else {
switch (_sentence_type) {
@ -325,6 +413,13 @@ bool AP_GPS_NMEA::_term_complete()
// the first term determines the sentence type
if (_term_number == 0) {
/*
special case for $PHD message
*/
if (strcmp(_term, "PHD") == 0) {
_sentence_type = _GPS_SENTENCE_PHD;
return false;
}
/*
The first two letters of the NMEA term are the talker
ID. The most common is 'GP' but there are a bunch of others
@ -424,6 +519,23 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_VTG + 1: // Course (VTG)
_new_course = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_PHD + 1: // PHD class
_phd.msg_class = atol(_term);
break;
case _GPS_SENTENCE_PHD + 2: // PHD message
_phd.msg_id = atol(_term);
if (_phd.msg_class == 1 && (_phd.msg_id == 12 || _phd.msg_id == 26)) {
// we only support $PHD messages 1/12 and 1/26
_gps_data_good = true;
}
break;
case _GPS_SENTENCE_PHD + 5: // PHD message, itow
_phd.itow = strtoul(_term, nullptr, 10);
break;
case _GPS_SENTENCE_PHD + 6 ... _GPS_SENTENCE_PHD + 11: // PHD message, fields
_phd.fields[_term_number-6] = atol(_term);
break;
}
}

36
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -67,11 +67,12 @@ public:
private:
/// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.
enum _sentence_types : uint8_t { //there are some more than 10 fields in some sentences , thus we have to increase these value.
_GPS_SENTENCE_RMC = 32,
_GPS_SENTENCE_GGA = 64,
_GPS_SENTENCE_VTG = 96,
_GPS_SENTENCE_HDT = 128,
_GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules
_GPS_SENTENCE_OTHER = 0
};
@ -114,6 +115,9 @@ private:
/// return true if we have a new set of NMEA messages
bool _have_new_message(void);
// print a formatted NMEA message to the port
bool nmea_printf(const char *fmt, ...) const;
uint8_t _parity; ///< NMEA message checksum accumulator
bool _is_checksum_term; ///< current term is the checksum
char _term[15]; ///< buffer for the current term within the current sentence
@ -139,10 +143,13 @@ private:
uint8_t _new_satellite_count; ///< satellite count parsed from a term
uint8_t _new_quality_indicator; ///< GPS quality indicator parsed from a term
uint32_t _last_RMC_ms = 0;
uint32_t _last_GGA_ms = 0;
uint32_t _last_VTG_ms = 0;
uint32_t _last_HDT_ms = 0;
uint32_t _last_RMC_ms;
uint32_t _last_GGA_ms;
uint32_t _last_VTG_ms;
uint32_t _last_HDT_ms;
uint32_t _last_PHD_12_ms;
uint32_t _last_PHD_26_ms;
uint32_t _last_fix_ms;
/// @name Init strings
/// In ::init, an attempt is made to configure the GPS
@ -155,6 +162,25 @@ private:
//@}
static const char _initialisation_blob[];
/*
the $PHD message is an extension from AllyStar that gives
vertical velocity and more accuracy estimates. It is designed as
a mapping from ublox UBX protocol messages to NMEA. So class 1,
message 12 is a mapping to NMEA of the NAV-VELNED UBX message
and contains the same fields. Class 1 message 26 is called
"NAV-PVERR", but does not correspond to a UBX message
example:
$PHD,01,12,TIIITTITT,,245808000,0,0,0,0,0,10260304,0,0*27
$PHD,01,26,TTTTTTT,,245808000,877,864,1451,11,11,17*17
*/
struct {
uint8_t msg_class;
uint8_t msg_id;
uint32_t itow;
int32_t fields[8];
} _phd;
};
#define AP_GPS_NMEA_HEMISPHERE_INIT_STRING \

5
libraries/AP_GPS/GPS_Backend.h
View File

@ -127,6 +127,11 @@ protected:
bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
#endif //GPS_MOVING_BASELINE
// get GPS type, for subtype config
AP_GPS::GPS_Type get_type() const {
return gps.get_type(state.instance);
}
private:
// itow from previous message
uint32_t _last_itow;