diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index ea77df3c8b..d05e0b61e5 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -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(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(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(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(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(pvt_geodetic.h_accuracy) / 200.0f; - _message_gps_state.epv = static_cast(pvt_geodetic.v_accuracy) / 200.0f; + _sensor_gps.eph = static_cast(pvt_geodetic.h_accuracy) / 200.0f; + _sensor_gps.epv = static_cast(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(epoch) * 1000000ULL; - _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + _sensor_gps.time_utc_usec = static_cast(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(att_cov_euler.error_aux1) == Error::None && static_cast(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) diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index d4f86a2a15..052d6ac1cd 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -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 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fee9b8b0f7..4e3ec68b64 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -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 _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; uORB::PublicationMulti _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); } } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 7f67ff7a27..d5b12aa609 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -356,39 +356,39 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure 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 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 } // 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(); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index f8205c3b38..2a5d7c06ff 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -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 };