fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Publish RTCM stream telemetry also for UAVCAN GNSS receivers (#25315)

Browse Source 
* uavcan gnss: publish rtcm instance and injection rate

* GPS drivers minor refactor (#25316)

* GPS drivers: adhere to message name based variable naming convention

* uavcan gnss: use matrix norm calculation

* GPS drivers: rate naming refactor and robust timeout checks

---------

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
Matthias Grob
2025-07-29 09:55:21 -08:00
committed by GitHub
co-authored by Jacob Dahl
parent b5bf28c204
commit 6cf494dde1
5 changed files with 149 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+36 -33
View File
@@ -87,7 +87,6 @@ using namespace time_literals;
#define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
#define TIMEOUT_5HZ 500 //!< Timeout time in mS, 200 mS (5Hz) + 300 mS delta for error
#define TIMEOUT_DUMP_ADD 450 //!< Additional time in mS to account for RTCM3 parsing and dumping
#define RATE_MEASUREMENT_PERIOD 5_s
enum class gps_driver_mode_t {
None = 0,
@@ -188,17 +187,17 @@ private:
GPS_Sat_Info *_sat_info{nullptr}; ///< instance of GPS sat info data object
sensor_gps_s _report_gps_pos{}; ///< uORB topic for gps position
sensor_gps_s _sensor_gps{}; ///< uORB topic for gps position
satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info
uORB::PublicationMulti<sensor_gps_s> _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position
uORB::PublicationMulti<sensor_gnss_relative_s> _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)};
uORB::PublicationMulti<satellite_info_s> _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info
float _rate{0.0f}; ///< position update rate
float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate
unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages
float _rtcm_injection_rate{0.0f}; ///< RTCM message injection rate
unsigned _rtcm_injection_rate_message_count{0}; ///< counter for number of RTCM messages
unsigned _num_bytes_read{0}; ///< counter for number of read bytes from the UART (within update interval)
unsigned _rate_reading{0}; ///< reading rate in B/s
hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection
@@ -312,8 +311,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = NAN;
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = NAN;
int32_t enable_sat_info = 0;
param_get(param_find("GPS_SAT_INFO"), &enable_sat_info);
@@ -540,8 +539,10 @@ void GPS::handleInjectDataTopic()
bool already_copied = false;
gps_inject_data_s msg;
const hrt_abstime now = hrt_absolute_time();
// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
if (now > _last_rtcm_injection_time + 5_s) {
for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
const bool exists = _orb_inject_data_sub[instance].advertised();
@@ -553,7 +554,7 @@ void GPS::handleInjectDataTopic()
*/
if (msg.device_id != get_device_id()) {
// Only use the message if it is up to date
if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
if (now < msg.timestamp + 5_s) {
// Remember that we already did a copy on this instance.
already_copied = true;
_selected_rtcm_instance = instance;
@@ -586,7 +587,7 @@ void GPS::handleInjectDataTopic()
*/
injectData(msg.data, msg.len);
++_last_rate_rtcm_injection_count;
++_rtcm_injection_rate_message_count;
_last_rtcm_injection_time = hrt_absolute_time();
}
}
@@ -853,34 +854,34 @@ GPS::run()
/* FALLTHROUGH */
case gps_driver_mode_t::UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info,
gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
#ifndef CONSTRAINED_FLASH
case gps_driver_mode_t::MTK:
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
_helper = new GPSDriverMTK(&GPS::callback, this, &_sensor_gps);
set_device_type(DRV_GPS_DEVTYPE_MTK);
break;
case gps_driver_mode_t::ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
_helper = new GPSDriverAshtech(&GPS::callback, this, &_sensor_gps, _p_report_sat_info, heading_offset);
set_device_type(DRV_GPS_DEVTYPE_ASHTECH);
break;
case gps_driver_mode_t::EMLIDREACH:
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_sensor_gps, _p_report_sat_info);
set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
break;
case gps_driver_mode_t::FEMTOMES:
_helper = new GPSDriverFemto(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
_helper = new GPSDriverFemto(&GPS::callback, this, &_sensor_gps, _p_report_sat_info, heading_offset);
set_device_type(DRV_GPS_DEVTYPE_FEMTOMES);
break;
case gps_driver_mode_t::NMEA:
_helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
_helper = new GPSDriverNMEA(&GPS::callback, this, &_sensor_gps, _p_report_sat_info, heading_offset);
set_device_type(DRV_GPS_DEVTYPE_NMEA);
break;
#endif // CONSTRAINED_FLASH
@@ -921,9 +922,9 @@ GPS::run()
if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = heading_offset;
memset(&_sensor_gps, 0, sizeof(_sensor_gps));
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = heading_offset;
if (_mode == gps_driver_mode_t::UBX) {
@@ -993,15 +994,17 @@ GPS::run()
reset_if_scheduled();
const hrt_abstime now = hrt_absolute_time();
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
if (now > last_rate_measurement + 5_s) {
float dt = (float)((now - last_rate_measurement)) / 1e6f;
_rate = last_rate_count / dt;
_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
_rtcm_injection_rate = _rtcm_injection_rate_message_count / dt;
_rate_reading = _num_bytes_read / dt;
last_rate_measurement = hrt_absolute_time();
last_rate_measurement = now;
last_rate_count = 0;
_last_rate_rtcm_injection_count = 0;
_rtcm_injection_rate_message_count = 0;
_num_bytes_read = 0;
_helper->storeUpdateRates();
_helper->resetUpdateRates();
@@ -1040,7 +1043,7 @@ GPS::run()
if (_healthy) {
_healthy = false;
_rate = 0.0f;
_rate_rtcm_injection = 0.0f;
_rtcm_injection_rate = 0.0f;
}
}
@@ -1146,16 +1149,16 @@ GPS::print_status()
PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
PX4_INFO("rate reading: \t\t%6i B/s", _rate_reading);
if (_report_gps_pos.timestamp != 0) {
if (_sensor_gps.timestamp != 0) {
if (_helper) {
PX4_INFO("rate position: \t\t%6.2f Hz", (double)_helper->getPositionUpdateRate());
PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate());
}
PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rtcm_injection_rate);
print_message(ORB_ID(sensor_gps), _report_gps_pos);
print_message(ORB_ID(sensor_gps), _sensor_gps);
}
if (_instance == Instance::Main && _secondary_instance.load()) {
@@ -1202,15 +1205,15 @@ void
GPS::publish()
{
if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
_report_gps_pos.device_id = get_device_id();
_sensor_gps.device_id = get_device_id();
_report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance;
_report_gps_pos.rtcm_injection_rate = _rate_rtcm_injection;
_sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
_sensor_gps.rtcm_injection_rate = _rtcm_injection_rate;
_report_gps_pos_pub.publish(_report_gps_pos);
_sensor_gps_pub.publish(_sensor_gps);
// Heading/yaw data can be updated at a lower rate than the other navigation data.
// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
_report_gps_pos.heading = NAN;
_sensor_gps.heading = NAN;
_is_gps_main_advertised.store(true);
}
}