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

AP_GPS: fixed re-probing of NMEA GPS 

when we lost fix on a NMEA GPS we should retain the driver as long as
it is still sending us NMEA messages that pass the CRC check. This
fixes a bug where we would re-probe the GPS (searching baudrates) when
we lost position
This commit is contained in:
Andrew Tridgell 2022-11-22 18:29:19 +11:00
parent ad9dfe2d51
commit 02d6931c4d
2 changed files with 166 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

362
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -118,7 +118,6 @@ bool AP_GPS_NMEA::_decode(char c)
_crc32 = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
_sentence_length = 1;
_sentence_done = false;
return false;
@ -293,195 +292,181 @@ bool AP_GPS_NMEA::_term_complete()
const uint32_t crc = strtoul(_term, nullptr, 16);
const bool crc_ok = _is_unicore? (_crc32 == crc) : (_parity == crc);
if (crc_ok) {
if (_gps_data_good) {
uint32_t now = AP_HAL::millis();
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
_last_RMC_ms = now;
//time = _new_time;
//date = _new_date;
if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
if (_last_3D_velocity_ms == 0 ||
now - _last_3D_velocity_ms > 1000) {
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
}
make_gps_time(_new_date, _new_time * 10);
set_uart_timestamp(_sentence_length);
state.last_gps_time_ms = now;
uint32_t now = AP_HAL::millis();
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
_last_RMC_ms = now;
//time = _new_time;
//date = _new_date;
if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
if (_last_3D_velocity_ms == 0 ||
now - _last_3D_velocity_ms > 1000) {
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
}
make_gps_time(_new_date, _new_time * 10);
set_uart_timestamp(_sentence_length);
state.last_gps_time_ms = now;
if (_last_vvelocity_ms == 0 ||
now - _last_vvelocity_ms > 1000) {
fill_3d_velocity();
}
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
state.location.alt = _new_altitude;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
state.num_sats = _new_satellite_count;
state.hdop = _new_hdop;
switch(_new_quality_indicator) {
case 0: // Fix not available or invalid
state.status = AP_GPS::NO_FIX;
break;
case 1: // GPS SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // Differential GPS, SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // GPS PPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // Float RTK. Satellite system used in RTK mode, floating integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // Estimated (dead reckoning) Mode
state.status = AP_GPS::NO_FIX;
break;
default://to maintain compatibility with MAV_GPS_INPUT and others
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
}
break;
case _GPS_SENTENCE_VTG:
_last_VTG_ms = now;
if (_last_3D_velocity_ms == 0 ||
now - _last_3D_velocity_ms > 1000) {
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
if (_last_vvelocity_ms == 0 ||
now - _last_vvelocity_ms > 1000) {
fill_3d_velocity();
}
}
// VTG has no fix indicator, can't change fix status
break;
case _GPS_SENTENCE_HDT:
case _GPS_SENTENCE_THS:
if (_last_AGRICA_ms != 0 || _expect_agrica) {
// use AGRICA
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
state.location.alt = _new_altitude;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
state.num_sats = _new_satellite_count;
state.hdop = _new_hdop;
switch(_new_quality_indicator) {
case 0: // Fix not available or invalid
state.status = AP_GPS::NO_FIX;
break;
case 1: // GPS SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // Differential GPS, SPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // GPS PPS Mode, fix valid
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // Float RTK. Satellite system used in RTK mode, floating integers
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // Estimated (dead reckoning) Mode
state.status = AP_GPS::NO_FIX;
break;
default://to maintain compatibility with MAV_GPS_INPUT and others
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
}
}
_last_yaw_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
// remember that we are setup to provide yaw. With
// a NMEA GPS we can only tell if the GPS is
// configured to provide yaw when it first sends a
// HDT sentence.
state.gps_yaw_configured = true;
break;
case _GPS_SENTENCE_PHD:
if (_last_AGRICA_ms != 0) {
// prefer AGRICA
break;
case _GPS_SENTENCE_VTG:
_last_VTG_ms = now;
if (_last_3D_velocity_ms == 0 ||
now - _last_3D_velocity_ms > 1000) {
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
if (_last_vvelocity_ms == 0 ||
now - _last_vvelocity_ms > 1000) {
fill_3d_velocity();
}
}
// VTG has no fix indicator, can't change fix status
}
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_vvelocity_ms = now;
// we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_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_vaccuracy_ms = now;
}
break;
case _GPS_SENTENCE_KSXT:
if (_last_AGRICA_ms != 0 || _expect_agrica) {
// prefer AGRICA
break;
case _GPS_SENTENCE_HDT:
case _GPS_SENTENCE_THS:
if (_last_AGRICA_ms != 0 || _expect_agrica) {
// use AGRICA
break;
}
}
state.location.lat = _ksxt.fields[2]*1.0e7;
state.location.lng = _ksxt.fields[1]*1.0e7;
state.location.alt = _ksxt.fields[3]*1.0e2;
_last_KSXT_pos_ms = now;
if (_ksxt.fields[9] >= 1) {
// we have 3D fix
constexpr float kmh_to_mps = 1.0 / 3.6;
state.velocity.y = _ksxt.fields[16] * kmh_to_mps;
state.velocity.x = _ksxt.fields[17] * kmh_to_mps;
state.velocity.z = _ksxt.fields[18] * -kmh_to_mps;
state.have_vertical_velocity = true;
_last_vvelocity_ms = now;
// we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_ms = now;
}
if (is_equal(3.0f, float(_ksxt.fields[10]))) {
// have good yaw (from RTK fixed moving baseline solution)
_last_yaw_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.gps_yaw = _ksxt.fields[4];
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
// remember that we are setup to provide yaw. With
// a NMEA GPS we can only tell if the GPS is
// configured to provide yaw when it first sends a
// HDT sentence.
state.gps_yaw_configured = true;
break;
case _GPS_SENTENCE_PHD:
if (_last_AGRICA_ms != 0) {
// prefer AGRICA
break;
}
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_vvelocity_ms = now;
// we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_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_vaccuracy_ms = now;
}
break;
case _GPS_SENTENCE_KSXT:
if (_last_AGRICA_ms != 0 || _expect_agrica) {
// prefer AGRICA
break;
}
state.location.lat = _ksxt.fields[2]*1.0e7;
state.location.lng = _ksxt.fields[1]*1.0e7;
state.location.alt = _ksxt.fields[3]*1.0e2;
_last_KSXT_pos_ms = now;
if (_ksxt.fields[9] >= 1) {
// we have 3D fix
constexpr float kmh_to_mps = 1.0 / 3.6;
state.velocity.y = _ksxt.fields[16] * kmh_to_mps;
state.velocity.x = _ksxt.fields[17] * kmh_to_mps;
state.velocity.z = _ksxt.fields[18] * -kmh_to_mps;
state.have_vertical_velocity = true;
_last_vvelocity_ms = now;
// we prefer a true 3D velocity when available
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = state.velocity.xy().length();
_last_3D_velocity_ms = now;
}
if (is_equal(3.0f, float(_ksxt.fields[10]))) {
// have good yaw (from RTK fixed moving baseline solution)
_last_yaw_ms = now;
state.gps_yaw = _ksxt.fields[4];
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
state.gps_yaw_configured = true;
}
break;
case _GPS_SENTENCE_AGRICA: {
auto &ag = _agrica;
_last_AGRICA_ms = now;
state.location.lat = ag.lat*1.0e7;
state.location.lng = ag.lng*1.0e7;
state.location.alt = ag.alt*1.0e2;
state.undulation = -ag.undulation;
state.velocity = ag.vel_NED;
state.speed_accuracy = ag.vel_stddev.length();
state.horizontal_accuracy = ag.pos_stddev.xy().length();
state.vertical_accuracy = ag.pos_stddev.z;
state.have_vertical_velocity = true;
state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
state.have_undulation = true;
if (ag.heading_status == 4) {
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;
calculate_moving_base_yaw(bearing, dist, alt_diff);
state.gps_yaw_configured = true;
} else {
state.have_gps_yaw = false;
}
check_new_itow(ag.itow, _sentence_length);
break;
}
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
case _GPS_SENTENCE_GGA:
// Only these sentences give us information about
// fix status.
state.status = AP_GPS::NO_FIX;
break;
case _GPS_SENTENCE_THS:
break;
case _GPS_SENTENCE_AGRICA: {
auto &ag = _agrica;
_last_AGRICA_ms = now;
state.location.lat = ag.lat*1.0e7;
state.location.lng = ag.lng*1.0e7;
state.location.alt = ag.alt*1.0e2;
state.undulation = -ag.undulation;
state.velocity = ag.vel_NED;
state.speed_accuracy = ag.vel_stddev.length();
state.horizontal_accuracy = ag.pos_stddev.xy().length();
state.vertical_accuracy = ag.pos_stddev.z;
state.have_vertical_velocity = true;
state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
state.have_undulation = true;
if (ag.heading_status == 4) {
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;
calculate_moving_base_yaw(bearing, dist, alt_diff);
state.gps_yaw_configured = true;
} else {
state.have_gps_yaw = false;
break;
}
check_new_itow(ag.itow, _sentence_length);
break;
}
}
// see if we got a good message
return _have_new_message();
@ -501,12 +486,10 @@ bool AP_GPS_NMEA::_term_complete()
}
if (strcmp(_term, "KSXT") == 0) {
_sentence_type = _GPS_SENTENCE_KSXT;
_gps_data_good = true;
return false;
}
if (strcmp(_term, "AGRICA") == 0) {
_sentence_type = _GPS_SENTENCE_AGRICA;
_gps_data_good = true;
return false;
}
/*
@ -526,15 +509,10 @@ bool AP_GPS_NMEA::_term_complete()
_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, "THS") == 0) {
_sentence_type = _GPS_SENTENCE_THS;
} else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
@ -547,17 +525,13 @@ bool AP_GPS_NMEA::_term_complete()
// operational status
//
case _GPS_SENTENCE_RMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
_new_quality_indicator = _term[0] - '0';
break;
case _GPS_SENTENCE_THS + 2: // validity (THS)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
if (_term[0] > '0') {
_new_quality_indicator = _term[0] - '0';
} else {
_new_quality_indicator = 0;
}
break;
case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
@ -622,10 +596,6 @@ bool AP_GPS_NMEA::_term_complete()
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);

1
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -134,7 +134,6 @@ private:
uint16_t _term_number; ///< term index within the current sentence
uint8_t _term_offset; ///< character offset with the term being received
uint16_t _sentence_length;
bool _gps_data_good; ///< set when the sentence indicates data is good
bool _sentence_done; ///< set when a sentence has been fully decoded
// The result of parsing terms within a message is stored temporarily until