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 againif (_last_VTG_ms != 0) {_last_VTG_ms = 1;}if (now - _last_HDT_ms > 300) {// we have lost GPS yawstate.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 alonereturn;}if (_type[instance] == GPS_TYPE_NONE) {// not enabledstate[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 driverreturn;}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-initialiseddetect_instance(instance);return;}if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {send_blob_update(instance);}// we have an active driver for this instancebool 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 againbool 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 MAVif (_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 timedelete 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 availabletnow = state[instance].uart_timestamp_ms;state[instance].uart_timestamp_ms = 0;}// delta will only be correct after parsing two messagestiming[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 > 1if (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 roverconst 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 roverinject_data(i, rtcm_data, rtcm_len);drivers[instance]->clear_RTCMV3();break;}}}}
#endifif (data_should_be_logged) {// keep count of delayed frames and average frame delay for health reportingconst uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms bufferGPS_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_PERIPHif (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 messageif (_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 invalidstate.status = AP_GPS::NO_FIX;break;case 1: // GPS SPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D;break;case 2: // Differential GPS, SPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D_DGPS;break;case 3: // GPS PPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D;break;case 4: // Real Time Kinematic. System used in RTK mode with fixed integersstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;break;case 5: // Float RTK. Satellite system used in RTK mode, floating integersstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;break;case 6: // Estimated (dead reckoning) Modestate.status = AP_GPS::NO_FIX;break;default://to maintain compatibility with MAV_GPS_INPUT and othersstate.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 statusbreak;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 messagereturn _have_new_message();}// we got a bad message, ignore itreturn false;}// the first term determines the sentence typeif (_term_number == 0) {/*The first two letters of the NMEA term are the talkerID. The most common is 'GP' but there are a bunch of othersthat 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 = HDTif (_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: // Latitudecase _GPS_SENTENCE_GGA + 2:_new_latitude = _parse_degrees();break;case _GPS_SENTENCE_RMC + 4: // N/Scase _GPS_SENTENCE_GGA + 3:if (_term[0] == 'S')_new_latitude = -_new_latitude;break;case _GPS_SENTENCE_RMC + 5: // Longitudecase _GPS_SENTENCE_GGA + 4:_new_longitude = _parse_degrees();break;case _GPS_SENTENCE_RMC + 6: // E/Wcase _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.514break;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 5Hzuint32_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 againif (_last_VTG_ms != 0) {_last_VTG_ms = 1;}if (now - _last_HDT_ms > 300) {// we have lost GPS yawstate.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 messageif (_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 invalidstate.status = AP_GPS::NO_FIX;break;case 1: // GPS SPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D;break;case 2: // Differential GPS, SPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D_DGPS;break;case 3: // GPS PPS Mode, fix validstate.status = AP_GPS::GPS_OK_FIX_3D;break;case 4: // Real Time Kinematic. System used in RTK mode with fixed integersstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;break;case 5: // Float RTK. Satellite system used in RTK mode, floating integersstate.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;break;case 6: // Estimated (dead reckoning) Modestate.status = AP_GPS::NO_FIX;break;default://to maintain compatibility with MAV_GPS_INPUT and othersstate.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 statusbreak;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 timecase _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 messagereturn _have_new_message();}// we got a bad message, ignore itreturn false;}// the first term determines the sentence typeif (_term_number == 0) {/*The first two letters of the NMEA term are the talkerID. The most common is 'GP' but there are a bunch of othersthat 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 = HDTif (_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: // Latitudecase _GPS_SENTENCE_GGA + 2:_new_latitude = _parse_degrees();break;case _GPS_SENTENCE_RMC + 4: // N/Scase _GPS_SENTENCE_GGA + 3:if (_term[0] == 'S')_new_latitude = -_new_latitude;break;case _GPS_SENTENCE_RMC + 5: // Longitudecase _GPS_SENTENCE_GGA + 4:_new_longitude = _parse_degrees();break;case _GPS_SENTENCE_RMC + 6: // E/Wcase _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.514break;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，然后又检测到的问题

Ardupilot-NMEA协议的GNSS处理优化相关推荐

Android GNSS 模块分析（五）NMEA 协议
紧接着上一篇<Android GNSS 模块分析(四)HAL 层>,本篇简述下导航硬件设备与卫星导航系统之间的通信协议. NMEA 协议简介: NMEA(National Marine ...
GPS NMEA协议解析之通用语句
GPS NMEA协议解析(NMEA通用语句) 文章目录前言一.NMEA协议简介二.NMEA数据格式 1.GGA(全球定位系统定位数据) 2.GSA(GNSS 精度因子与有效卫星) 3.GSV(可 ...
【灵动MM32-DMA传输-GPS解算】移植NMEA协议库解析GGA数据格式
灵动MM32单片机移植NMEA协议库解算GGA数据格式通过串口dma硬件传输今天使用一款常见的gps模块,goouuu果云GPS模块,这款产品可以说是便宜好用,但是这个原版本的例程我觉得不太行,解析 ...
嵌入式GPS模块编程 NMEA协议 0183协议
嵌入式GPS模块编程 NMEA协议 0183协议学前小知识: NMEA协议是为了在不同的GPS导航设备中建立统一的RTCM(海事无线电技术委员会)标准,它最初是由美国国家海洋电子协会(NMEA-Th ...
大型网站的HTTPS实践：基于协议和配置的优化
作者 | 百度HTTPS技术支持团队百度已经上线了全站 HTTPS 的安全搜索,默认会将 HTTP 请求跳转成 HTTPS.百度 HTTPS性能优化涉及到大量内容,在前端页面.后端架构.协议特性.加 ...
GPS卫星定位接收器的NMEA协议解析
原文地址:GPS卫星定位接收器的NMEA协议解析作者:蟹蟹 GPS接收机只要处于工作状态就会源源不断地把接收并计算出的GPS导航定位信息通过串口传送到计算机中.前面的代码只负责从串口接收数据并将其放置 ...
Cirium（睿思誉）与Amadeus达成协议，助力旅游业优化航班搜索和预订
Amadeus将Cirium(睿思誉)先进的航班计划数据整合到自身旅游平台解决方案中,丰富了航班和搜索预订的可用数据集. 该协议延长了两个行业领导者之间长达十年的合作伙伴关系新数据访问权限的开通可进 ...
GPS数据类型格式 NMEA协议
转载:原文链接 GPS数据类型格式数据类型类别描述 GPGSV 可见卫星信息 GPRMC 推荐最小定位信息 GPVTG 地面速度信息 GPGGA GPS定位信息 GPGSA 当前卫星信息数据格 ...
APM-GPS数据解析(NMEA协议)
一.概括 NEMA报文有GPGGA,GPRMC,GNHDT,等格式.每句报文以$开始,以*结束.*后面的两位数字是校验和.其中校验和以$,*之间所有字符异或得到,不包含$和*. 二.取数将串口读到的 ...

Ardupilot-NMEA协议的GNSS处理优化

Ardupilot-NMEA协议的GNSS处理优化

Ardupilot-NMEA协议的GNSS处理优化

原固件存在的问题

解决办法

Ardupilot-NMEA协议的GNSS处理优化相关推荐

最新文章

热门文章