Ardupilot-NMEA协议的GNSS处理优化
Ardupilot-NMEA协议的GNSS处理优化
原固件存在的问题
1、当所使用的GNSS模块数据反馈频率不到10Hz时，存在锁定GNSS一会又自动把它删除。
原因如下：检测新消息需要判断RMC 和GGA消息包不大于150ms，返回false，然后在update_instance函数中进入超时未收到数据包而误判无GNSS，所有把端口删除， delete drivers[instance]
/*
see if we have a new set of NMEA messages
*/
bool AP_GPS_NMEA::_have_new_message()
{
if (_last_RMC_ms == 0 ||
_last_GGA_ms == 0) {
return false;
}
uint32_t now = AP_HAL::millis();
if (now - _last_RMC_ms > 150 ||
now - _last_GGA_ms > 150) {
return false;
}
if (_last_VTG_ms != 0 &&
now - _last_VTG_ms > 150) {
return false;
}
// prevent these messages being used again
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;
}
/*
update one GPS instance. This should be called at 10Hz or greater
*/
void AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) {
// in HIL, leave info alone
return;
}
if (_type[instance] == GPS_TYPE_NONE) {
// not enabled
state[instance].status = NO_GPS;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
return;
}
if (locked_ports & (1U<<instance)) {
// the port is locked by another driver
return;
}
if (drivers[instance] == nullptr) {
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance(instance);
return;
}
if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
send_blob_update(instance);
}
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
bool data_should_be_logged = false;
if (!result) {
if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
memset((void *)&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
timing[instance].last_message_time_ms = tnow;
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
// do not try to detect again if type is MAV
if (_type[instance] == GPS_TYPE_MAV) {
state[instance].status = NO_FIX;
} else {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = nullptr;
state[instance].status = NO_GPS;
}
// log this data as a "flag" that the GPS is no longer
// valid (see PR#8144)
data_should_be_logged = true;
}
} else {
if (state[instance].uart_timestamp_ms != 0) {
// set the timestamp for this messages based on
// set_uart_timestamp() in backend, if available
tnow = state[instance].uart_timestamp_ms;
state[instance].uart_timestamp_ms = 0;
}
// delta will only be correct after parsing two messages
timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
data_should_be_logged = true;
}
#if GPS_MAX_RECEIVERS > 1
if (drivers[instance] && _type[instance] == GPS_TYPE_UBLOX_RTK_BASE) {
// see if a moving baseline base has some RTCMv3 data
// which we need to pass along to the rover
const uint8_t *rtcm_data;
uint16_t rtcm_len;
if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (i != instance && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
// pass the data to the rover
inject_data(i, rtcm_data, rtcm_len);
drivers[instance]->clear_RTCMV3();
break;
}
}
}
}
#endif
if (data_should_be_logged) {
// keep count of delayed frames and average frame delay for health reporting
const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
GPS_timing &t = timing[instance];
if (t.delta_time_ms > gps_max_delta_ms) {
t.delayed_count++;
} else {
t.delayed_count = 0;
}
if (t.delta_time_ms < 2000) {
if (t.average_delta_ms <= 0) {
t.average_delta_ms = t.delta_time_ms;
} else {
t.average_delta_ms = 0.98f * t.average_delta_ms + 0.02f * t.delta_time_ms;
}
}
}
#ifndef HAL_BUILD_AP_PERIPH
if (data_should_be_logged &&
(should_log() || AP::ahrs().have_ekf_logging())) {
AP::logger().Write_GPS(instance);
}
if (state[instance].status >= GPS_OK_FIX_3D) {
const uint64_t now = time_epoch_usec(instance);
if (now != 0) {
AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS);
}
}
#else
(void)data_should_be_logged;
#endif
}
2、GNSS未定位时不更新收到RMC消息包的时间戳_last_RMC_ms
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{
// handle the last term in a message
if (_is_checksum_term) {
uint8_t nibble_high = 0;
uint8_t nibble_low = 0;
if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) {
return false;
}
const uint8_t checksum = (nibble_high << 4u) | nibble_low;
if (checksum == _parity) {
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;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
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;
fill_3d_velocity();
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
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;
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
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;
// 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;
}
} 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;
}
}
// see if we got a good message
return _have_new_message();
}
// we got a bad message, ignore it
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
/*
The first two letters of the NMEA term are the talker
ID. The most common is 'GP' but there are a bunch of others
that are valid. We accept any two characters here.
*/
if (_term[0] < 'A' || _term[0] > 'Z' ||
_term[1] < 'A' || _term[1] > 'Z') {
_sentence_type = _GPS_SENTENCE_OTHER;
return false;
}
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0) {
_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
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// 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_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
_new_hdop = (uint16_t)_parse_decimal_100(_term);
break;
// time and date
//
case _GPS_SENTENCE_RMC + 1: // Time (RMC)
case _GPS_SENTENCE_GGA + 1: // Time (GGA)
_new_time = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// location
//
case _GPS_SENTENCE_RMC + 3: // Latitude
case _GPS_SENTENCE_GGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 4: // N/S
case _GPS_SENTENCE_GGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_RMC + 5: // Longitude
case _GPS_SENTENCE_GGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 6: // E/W
case _GPS_SENTENCE_GGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal_100(_term);
break;
// course and speed
//
case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
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);
break;
}
}
return false;
}
解决办法
1、在 bool AP_GPS_NMEA::_have_new_message() 函数中把消息包更新间隔时间允许不大于250ms，可认为是支持5Hz数据输出的GNSS。
/*
see if we have a new set of NMEA messages
*/
bool AP_GPS_NMEA::_have_new_message()
{
if (_last_RMC_ms == 0 ||
_last_GGA_ms == 0) {
return false;
}
//GNSS is required to reach a frequency of at least 5Hz
uint32_t now = AP_HAL::millis();
if (now - _last_RMC_ms > 250 ||
now - _last_GGA_ms > 250) {
return false;
}
if (_last_VTG_ms != 0 &&
now - _last_VTG_ms > 250) {
return false;
}
// prevent these messages being used again
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;
}
2、未定位时添加RMS和GGA收包时间
将这两行加入_term_complete()函数
_last_RMC_ms = AP_HAL::millis(); //Correct package received when not located, refresh time
_last_GGA_ms = AP_HAL::millis(); //Correct package received when not located, refresh time
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{
// handle the last term in a message
if (_is_checksum_term) {
uint8_t nibble_high = 0;
uint8_t nibble_low = 0;
if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) {
return false;
}
const uint8_t checksum = (nibble_high << 4u) | nibble_low;
if (checksum == _parity) {
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;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
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;
fill_3d_velocity();
break;
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
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;
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
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;
// 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;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
_last_RMC_ms = AP_HAL::millis(); //Correct package received when not located, refresh time
case _GPS_SENTENCE_GGA:
_last_GGA_ms = AP_HAL::millis(); //Correct package received when not located, refresh time
// Only these sentences give us information about
// fix status.
state.status = AP_GPS::NO_FIX;
}
}
// see if we got a good message
return _have_new_message();
}
// we got a bad message, ignore it
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
/*
The first two letters of the NMEA term are the talker
ID. The most common is 'GP' but there are a bunch of others
that are valid. We accept any two characters here.
*/
if (_term[0] < 'A' || _term[0] > 'Z' ||
_term[1] < 'A' || _term[1] > 'Z') {
_sentence_type = _GPS_SENTENCE_OTHER;
return false;
}
const char *term_type = &_term[2];
if (strcmp(term_type, "RMC") == 0) {
_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
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// 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_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
_new_hdop = (uint16_t)_parse_decimal_100(_term);
break;
// time and date
//
case _GPS_SENTENCE_RMC + 1: // Time (RMC)
case _GPS_SENTENCE_GGA + 1: // Time (GGA)
_new_time = _parse_decimal_100(_term);
break;
case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// location
//
case _GPS_SENTENCE_RMC + 3: // Latitude
case _GPS_SENTENCE_GGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 4: // N/S
case _GPS_SENTENCE_GGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_RMC + 5: // Longitude
case _GPS_SENTENCE_GGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_RMC + 6: // E/W
case _GPS_SENTENCE_GGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal_100(_term);
break;
// course and speed
//
case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
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);
break;
}
}
return false;
}
由此解决了频率5Hz的GNSS使用NMEA协议时，出现的连接上GNSS后过4秒又出现NO GPS，然后又检测到的问题
1118
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
