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

AP_GPS: support UNIHEADINGA message for Unicore NMEA

Browse Source 
this allows for good yaw even without a RTK fix, which makes yaw with
dual-antenna Unicore GPS modules much more useful
This commit is contained in:
Andrew Tridgell 2022-12-27 12:18:01 +11:00 committed by Peter Barker
parent cf2d1ec290
commit 17c25780c7
2 changed files with 88 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -470,26 +470,6 @@ bool AP_GPS_NMEA::_term_complete()
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
state.have_undulation = true;
if (ag.heading_status == 4) {
#if GPS_MOVING_BASELINE
Location slave( ag.slave_lat*1.0e7, ag.slave_lng*1.0e7, ag.slave_alt*1.0e2, Location::AltFrame::ABSOLUTE );
const float dist = state.location.get_distance_NED(slave).length();
const float bearing = degrees(state.location.get_bearing(slave));
const float alt_diff = (state.location.alt - slave.alt)*0.01;
state.relPosHeading = bearing;
state.relPosLength = dist;
state.relPosD = alt_diff;
state.relposheading_ts = now;
if (calculate_moving_base_yaw(bearing, dist, alt_diff)) {
_last_yaw_ms = now;
}
state.gps_yaw_configured = true;
#endif
} else {
state.have_gps_yaw = false;
}
check_new_itow(ag.itow, _sentence_length);
break;
}
@ -502,6 +482,31 @@ bool AP_GPS_NMEA::_term_complete()
_versiona.build_date);
break;
}
case _GPS_SENTENCE_UNIHEADINGA: {
#if GPS_MOVING_BASELINE
const auto &ag = _agrica;
const auto &uh = _uniheadinga;
if (now - _last_AGRICA_ms > 500 || ag.heading_status != 4) {
// we need heading_status from AGRICA
state.have_gps_yaw = false;
break;
}
const float dist = uh.baseline_length;
const float bearing = uh.heading;
const float alt_diff = dist*tanf(radians(-uh.pitch));
state.relPosHeading = bearing;
state.relPosLength = dist;
state.relPosD = alt_diff;
state.relposheading_ts = now;
if (calculate_moving_base_yaw(bearing, dist, alt_diff)) {
state.have_gps_yaw_accuracy = true;
state.gps_yaw_accuracy = uh.heading_sd;
_last_yaw_ms = now;
}
state.gps_yaw_configured = true;
#endif // GPS_MOVING_BASELINE
break;
}
#endif // AP_GPS_NMEA_UNICORE_ENABLED
}
// see if we got a good message
@ -533,6 +538,10 @@ bool AP_GPS_NMEA::_term_complete()
_sentence_type = _GPS_SENTENCE_VERSIONA;
return false;
}
if (strcmp(_term, "UNIHEADINGA") == 0 && _expect_agrica) {
_sentence_type = _GPS_SENTENCE_UNIHEADINGA;
return false;
}
#endif
/*
The first two letters of the NMEA term are the talker
@ -655,6 +664,11 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_VERSIONA + 1 ... _GPS_SENTENCE_VERSIONA + 20:
parse_versiona_field(_term_number, _term);
break;
#if GPS_MOVING_BASELINE
case _GPS_SENTENCE_UNIHEADINGA + 1 ... _GPS_SENTENCE_UNIHEADINGA + 28: // UNIHEADINGA message
parse_uniheadinga_field(_term_number, _term);
break;
#endif
#endif
}
}
@ -708,15 +722,6 @@ void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term)
case 33:
ag.alt = atof(term);
break;
case 46:
ag.slave_lat = atof(term);
break;
case 47:
ag.slave_lng = atof(term);
break;
case 48:
ag.slave_alt = atof(term);
break;
case 49:
ag.itow = atol(term);
break;
@ -729,6 +734,41 @@ void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term)
}
}
#if GPS_MOVING_BASELINE
/*
parse a UNIHEADINGA message term
Example:
#UNIHEADINGA,79,GPS,FINE,2242,167498200,0,0,18,22;SOL_COMPUTED,L1_INT,2.7889,296.7233,-25.7710,0.0000,0.1127,0.1812,"999",49,37,37,0,3,00,1,51*d50af0ea
*/
void AP_GPS_NMEA::parse_uniheadinga_field(uint16_t term_number, const char *term)
{
const uint8_t hdr_align = 8;
if (term_number < hdr_align) {
// discard header;
return;
}
term_number -= hdr_align;
// useful for debugging
// ::printf("UNIHEADINGA[%u]=%s\n", unsigned(term_number), term);
auto &uh = _uniheadinga;
switch (term_number) {
case 4:
uh.baseline_length = atof(term);
break;
case 5:
uh.heading = atof(term);
break;
case 6:
uh.pitch = atof(term);
break;
case 8:
uh.heading_sd = atof(term);
break;
}
}
#endif // GPS_MOVING_BASELINE
// parse VERSIONA fields
void AP_GPS_NMEA::parse_versiona_field(uint16_t term_number, const char *term)
{
@ -823,7 +863,7 @@ void AP_GPS_NMEA::send_config(void)
"CONFIG HEADING FIXLENGTH\r\n" \
"CONFIG UNDULATION AUTO\r\n" \
"CONFIG\r\n" \
"GPGGAH %.3f\r\n", // needed to get slave antenna position in AGRICA
"UNIHEADINGA %.3f\r\n",
rate_s);
state.gps_yaw_configured = true;
FALLTHROUGH;

21
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -85,7 +85,8 @@ private:
_GPS_SENTENCE_THS = 160, // True heading with quality indicator, available on Trimble MB-Two
_GPS_SENTENCE_KSXT = 170, // extension for Unicore, 21 fields
_GPS_SENTENCE_AGRICA = 193, // extension for Unicore, 65 fields
_GPS_SENTENCE_VERSIONA = 270, // extension for Unicore, version
_GPS_SENTENCE_VERSIONA = 270, // extension for Unicore, version, 10 fields
_GPS_SENTENCE_UNIHEADINGA = 290, // extension for Unicore, uniheadinga, 20 fields
_GPS_SENTENCE_OTHER = 0
};
@ -136,6 +137,11 @@ private:
// parse VERSIONA field
void parse_versiona_field(uint16_t term_number, const char *term);
#if GPS_MOVING_BASELINE
// parse UNIHEADINGA field
void parse_uniheadinga_field(uint16_t term_number, const char *term);
#endif
#endif
@ -228,10 +234,10 @@ private:
float alt;
uint32_t itow;
float undulation;
double slave_lat, slave_lng;
float slave_alt;
Vector3f pos_stddev;
} _agrica;
// unicore VERSIONA parsing
struct {
char type[10];
char version[20];
@ -239,6 +245,15 @@ private:
} _versiona;
bool _have_unicore_versiona;
#if GPS_MOVING_BASELINE
// unicore UNIHEADINGA parsing
struct {
float baseline_length;
float heading;
float pitch;
float heading_sd;
} _uniheadinga;
#endif
#endif // AP_GPS_NMEA_UNICORE_ENABLED
bool _expect_agrica;