Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 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 19:55:21 +02:00
committed by GitHub
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/gnss/septentrio/septentrio.cpp
+47 -46
View File
@@ -249,7 +249,7 @@ int SeptentrioDriver::print_status()
if (first_gps_uorb_message_created() && _state == State::ReceivingData) {
PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
print_message(ORB_ID(sensor_gps), _message_gps_state);
print_message(ORB_ID(sensor_gps), _sensor_gps);
}
if (_instance == Instance::Main && secondary_instance) {
@@ -1057,8 +1057,8 @@ int SeptentrioDriver::process_message()
DOP dop;
if (_sbf_decoder.parse(&dop) == PX4_OK) {
_message_gps_state.hdop = dop.h_dop * 0.01f;
_message_gps_state.vdop = dop.v_dop * 0.01f;
_sensor_gps.hdop = dop.h_dop * 0.01f;
_sensor_gps.vdop = dop.v_dop * 0.01f;
result = PX4_OK;
}
@@ -1077,71 +1077,71 @@ int SeptentrioDriver::process_message()
if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) {
switch (static_cast<sbf::PVTGeodetic::ModeType>(pvt_geodetic.mode_type)) {
case ModeType::NoPVT:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_NONE;
break;
case ModeType::PVTWithSBAS:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
break;
case ModeType::RTKFloat:
case ModeType::MovingBaseRTKFloat:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
break;
case ModeType::RTKFixed:
case ModeType::MovingBaseRTKFixed:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
break;
default:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_3D;
break;
}
// Check boundaries and invalidate GPS velocities
if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) {
_message_gps_state.vel_ned_valid = false;
_sensor_gps.vel_ned_valid = false;
}
if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) {
_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
_sensor_gps.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
_sensor_gps.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
_sensor_gps.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
_sensor_gps.altitude_ellipsoid_m = pvt_geodetic.height;
} else {
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
_sensor_gps.fix_type = sensor_gps_s::FIX_TYPE_NONE;
}
if (pvt_geodetic.nr_sv != PVTGeodetic::k_dnu_nr_sv) {
_message_gps_state.satellites_used = pvt_geodetic.nr_sv;
_sensor_gps.satellites_used = pvt_geodetic.nr_sv;
if (_message_satellite_info) {
// Only fill in the satellite count for now (we could use the ChannelStatus message for the
// other data, but it's really large: >800B)
_message_satellite_info->timestamp = hrt_absolute_time();
_message_satellite_info->count = _message_gps_state.satellites_used;
_message_satellite_info->count = _sensor_gps.satellites_used;
}
} else {
_message_gps_state.satellites_used = 0;
_sensor_gps.satellites_used = 0;
}
/* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS.
* Divide by 100 from cm to m and in addition divide by 2 to get RMS. */
_message_gps_state.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
_message_gps_state.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;
_sensor_gps.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
_sensor_gps.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;
// Check fix and error code
_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
_message_gps_state.vel_n_m_s = pvt_geodetic.vn;
_message_gps_state.vel_e_m_s = pvt_geodetic.ve;
_message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu;
_message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s +
_message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s);
_sensor_gps.vel_ned_valid = _sensor_gps.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
_sensor_gps.vel_n_m_s = pvt_geodetic.vn;
_sensor_gps.vel_e_m_s = pvt_geodetic.ve;
_sensor_gps.vel_d_m_s = -1.0f * pvt_geodetic.vu;
_sensor_gps.vel_m_s = sqrtf(_sensor_gps.vel_n_m_s * _sensor_gps.vel_n_m_s +
_sensor_gps.vel_e_m_s * _sensor_gps.vel_e_m_s);
if (pvt_geodetic.cog > k_dnu_f4_value) {
_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
_sensor_gps.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
}
_message_gps_state.c_variance_rad = M_DEG_TO_RAD_F;
_sensor_gps.c_variance_rad = M_DEG_TO_RAD_F;
_message_gps_state.time_utc_usec = 0;
_sensor_gps.time_utc_usec = 0;
#ifndef __PX4_QURT // NOTE: Functionality isn't available on Snapdragon yet.
if (_time_synced) {
struct tm timeinfo;
@@ -1170,13 +1170,13 @@ int SeptentrioDriver::process_message()
ts.tv_nsec = (header.tow % 1000) * 1000 * 1000;
set_clock(ts);
_message_gps_state.time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_message_gps_state.time_utc_usec += (header.tow % 1000) * 1000;
_sensor_gps.time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_sensor_gps.time_utc_usec += (header.tow % 1000) * 1000;
}
}
#endif
_message_gps_state.timestamp = hrt_absolute_time();
_sensor_gps.timestamp = hrt_absolute_time();
result = PX4_OK;
}
@@ -1189,7 +1189,7 @@ int SeptentrioDriver::process_message()
ReceiverStatus receiver_status;
if (_sbf_decoder.parse(&receiver_status) == PX4_OK) {
_message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED;
_sensor_gps.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED;
_time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set;
}
@@ -1222,7 +1222,7 @@ int SeptentrioDriver::process_message()
if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) {
_message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
_sensor_gps.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
}
}
@@ -1252,7 +1252,7 @@ int SeptentrioDriver::process_message()
heading -= 2.f * M_PI_F;
}
_message_gps_state.heading = heading;
_sensor_gps.heading = heading;
}
break;
@@ -1270,7 +1270,7 @@ int SeptentrioDriver::process_message()
static_cast<AttCovEuler::Error>(att_cov_euler.error_aux1) == Error::None &&
static_cast<AttCovEuler::Error>(att_cov_euler.error_aux2) == Error::None &&
att_cov_euler.cov_headhead > k_dnu_f4_value) {
_message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
_sensor_gps.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
}
break;
@@ -1475,15 +1475,16 @@ void SeptentrioDriver::handle_inject_data_topic()
bool already_copied = false;
gps_inject_data_s msg;
// 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) {
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 (now > _last_rtcm_injection_time + 5_s) {
for (int instance = 0; instance < _gps_inject_data_sub.size(); instance++) {
const bool exists = _gps_inject_data_sub[instance].advertised();
if (exists) {
if (_gps_inject_data_sub[instance].copy(&msg)) {
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;
@@ -1528,10 +1529,10 @@ void SeptentrioDriver::handle_inject_data_topic()
void SeptentrioDriver::publish()
{
_message_gps_state.device_id = get_device_id();
_message_gps_state.selected_rtcm_instance = _selected_rtcm_instance;
_message_gps_state.rtcm_injection_rate = rtcm_injection_frequency();
_sensor_gps_pub.publish(_message_gps_state);
_sensor_gps.device_id = get_device_id();
_sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
_sensor_gps.rtcm_injection_rate = rtcm_injection_frequency();
_sensor_gps_pub.publish(_sensor_gps);
}
void SeptentrioDriver::publish_satellite_info()
@@ -1543,7 +1544,7 @@ void SeptentrioDriver::publish_satellite_info()
bool SeptentrioDriver::first_gps_uorb_message_created() const
{
return _message_gps_state.timestamp != 0;
return _sensor_gps.timestamp != 0;
}
void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
@@ -1696,9 +1697,9 @@ bool SeptentrioDriver::is_healthy() const
void SeptentrioDriver::reset_gps_state_message()
{
memset(&_message_gps_state, 0, sizeof(_message_gps_state));
_message_gps_state.heading = NAN;
_message_gps_state.heading_offset = matrix::wrap_pi(math::radians(_heading_offset));
memset(&_sensor_gps, 0, sizeof(_sensor_gps));
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = matrix::wrap_pi(math::radians(_heading_offset));
}
uint32_t SeptentrioDriver::get_parameter(const char *name, int32_t *value)
src/drivers/gnss/septentrio/septentrio.h
+1 -1
View File
@@ -737,7 +737,7 @@ private:
rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder
// uORB topics and subscriptions
sensor_gps_s _message_gps_state {}; ///< uORB topic for position
sensor_gps_s _sensor_gps{}; ///< uORB topic for position
gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver
gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver
satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info
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);
}
}
src/drivers/uavcan/sensors/gnss.cpp
+61 -49
View File
@@ -356,39 +356,39 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
const int32_t jamming_indicator, const uint8_t jamming_state,
const uint8_t spoofing_state)
{
sensor_gps_s report{};
report.device_id = get_device_id();
sensor_gps_s sensor_gps{};
sensor_gps.device_id = get_device_id();
/*
* FIXME HACK
* There used to be the following line of code:
* report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
* sensor_gps.timestamp_position = msg.getMonotonicTimestamp().toUSec();
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
* to use an independent time source (based on hardware TIM5) instead of HRT.
* The proper solution is to be developed.
*/
report.timestamp = hrt_absolute_time();
sensor_gps.timestamp = hrt_absolute_time();
report.latitude_deg = msg.latitude_deg_1e8 / 1e8;
report.longitude_deg = msg.longitude_deg_1e8 / 1e8;
report.altitude_msl_m = msg.height_msl_mm / 1e3;
report.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3;
sensor_gps.latitude_deg = msg.latitude_deg_1e8 / 1e8;
sensor_gps.longitude_deg = msg.longitude_deg_1e8 / 1e8;
sensor_gps.altitude_msl_m = msg.height_msl_mm / 1e3;
sensor_gps.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3;
if (valid_pos_cov) {
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
sensor_gps.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
sensor_gps.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
sensor_gps.eph = -1.0F;
sensor_gps.epv = -1.0F;
}
if (valid_vel_cov) {
report.s_variance_m_s = math::max(vel_cov[0], vel_cov[4], vel_cov[8]);
sensor_gps.s_variance_m_s = math::max(vel_cov[0], vel_cov[4], vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
@@ -404,46 +404,44 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
sensor_gps.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
sensor_gps.s_variance_m_s = -1.0F;
sensor_gps.c_variance_rad = -1.0F;
}
report.fix_type = fix_type;
sensor_gps.fix_type = fix_type;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s +
report.vel_e_m_s * report.vel_e_m_s +
report.vel_d_m_s * report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
sensor_gps.vel_n_m_s = msg.ned_velocity[0];
sensor_gps.vel_e_m_s = msg.ned_velocity[1];
sensor_gps.vel_d_m_s = msg.ned_velocity[2];
sensor_gps.vel_m_s = matrix::Vector3f(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]).norm();
sensor_gps.cog_rad = atan2f(sensor_gps.vel_e_m_s, sensor_gps.vel_n_m_s);
sensor_gps.vel_ned_valid = true;
report.timestamp_time_relative = 0;
sensor_gps.timestamp_time_relative = 0;
const uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
switch (msg.gnss_time_standard) {
case FixType::GNSS_TIME_STANDARD_UTC:
report.time_utc_usec = gnss_ts_usec;
sensor_gps.time_utc_usec = gnss_ts_usec;
break;
case FixType::GNSS_TIME_STANDARD_GPS:
if (msg.num_leap_seconds > 0) {
report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
sensor_gps.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
}
break;
case FixType::GNSS_TIME_STANDARD_TAI:
if (msg.num_leap_seconds > 0) {
report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
sensor_gps.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
}
break;
@@ -453,55 +451,58 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
}
// If we haven't already done so, set the system clock using GPS data
if (report.time_utc_usec != 0 && (fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
if (sensor_gps.time_utc_usec != 0 && (fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
timespec ts{};
// get the whole microseconds
ts.tv_sec = report.time_utc_usec / 1000000ULL;
ts.tv_sec = sensor_gps.time_utc_usec / 1000000ULL;
// get the remainder microseconds and convert to nanoseconds
ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000;
ts.tv_nsec = (sensor_gps.time_utc_usec % 1000000ULL) * 1000;
px4_clock_settime(CLOCK_REALTIME, &ts);
_system_clock_set = true;
}
report.satellites_used = msg.sats_used;
sensor_gps.satellites_used = msg.sats_used;
if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) {
report.hdop = _last_gnss_auxiliary_hdop;
report.vdop = _last_gnss_auxiliary_vdop;
sensor_gps.hdop = _last_gnss_auxiliary_hdop;
sensor_gps.vdop = _last_gnss_auxiliary_vdop;
} else {
// Using PDOP for HDOP and VDOP
// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
report.hdop = msg.pdop;
report.vdop = msg.pdop;
sensor_gps.hdop = msg.pdop;
sensor_gps.vdop = msg.pdop;
}
// Use heading from RelPosHeading message if available and we have RTK Fixed solution.
if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) {
report.heading = _rel_heading;
report.heading_offset = NAN;
report.heading_accuracy = _rel_heading_accuracy;
sensor_gps.heading = _rel_heading;
sensor_gps.heading_offset = NAN;
sensor_gps.heading_accuracy = _rel_heading_accuracy;
_rel_heading = NAN;
_rel_heading_accuracy = NAN;
_rel_heading_valid = false;
} else {
report.heading = heading;
report.heading_offset = heading_offset;
report.heading_accuracy = heading_accuracy;
sensor_gps.heading = heading;
sensor_gps.heading_offset = heading_offset;
sensor_gps.heading_accuracy = heading_accuracy;
}
report.noise_per_ms = noise_per_ms;
report.jamming_indicator = jamming_indicator;
report.jamming_state = jamming_state;
report.spoofing_state = spoofing_state;
sensor_gps.noise_per_ms = noise_per_ms;
sensor_gps.jamming_indicator = jamming_indicator;
sensor_gps.jamming_state = jamming_state;
sensor_gps.spoofing_state = spoofing_state;
publish(msg.getSrcNodeID().get(), &report);
sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
sensor_gps.rtcm_injection_rate = _rtcm_injection_rate;
publish(msg.getSrcNodeID().get(), &sensor_gps);
}
void UavcanGnssBridge::update()
@@ -516,13 +517,23 @@ void UavcanGnssBridge::update()
// to work.
void UavcanGnssBridge::handleInjectDataTopic()
{
hrt_abstime now = hrt_absolute_time();
// measure RTCM update rate every 5 seconds
if (now > _last_rate_measurement + 5_s) {
float dt = (now - _last_rate_measurement) / 1e6f;
_rtcm_injection_rate = _rtcm_injection_rate_message_count / dt;
_last_rate_measurement = now;
_rtcm_injection_rate_message_count = 0;
}
// We don't want to call copy again further down if we have already done a
// copy in the selection process.
bool already_copied = false;
gps_inject_data_s msg;
// 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();
@@ -565,6 +576,7 @@ void UavcanGnssBridge::handleInjectDataTopic()
PublishMovingBaselineData(msg.data, msg.len);
}
++_rtcm_injection_rate_message_count;
_last_rtcm_injection_time = hrt_absolute_time();
}
src/drivers/uavcan/sensors/gnss.hpp
+4
View File
@@ -150,4 +150,8 @@ private:
perf_counter_t _rtcm_stream_pub_perf{nullptr};
perf_counter_t _moving_baseline_data_pub_perf{nullptr};
hrt_abstime _last_rate_measurement{0};
float _rtcm_injection_rate{0.f}; ///< RTCM message injection rate
unsigned _rtcm_injection_rate_message_count{0}; ///< number of RTCM messages since last rate calculation
};