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

uavcan: subscriber for gnss.quality

Browse Source
This commit is contained in:
Jacob Dahl
2026-02-09 00:58:42 -09:00
parent 4240d0ee53
commit 1ee938863a
3 changed files with 41 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/libdronecan/dsdl
+1 -1
Submodule src/drivers/uavcan/libdronecan/dsdl updated: 993be80a62...6035657d00
src/drivers/uavcan/sensors/gnss.cpp
+30
View File
@@ -57,6 +57,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_
UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps), node_info_publisher),
_node(node),
_sub_auxiliary(node),
_sub_quality(node),
_sub_fix(node),
_sub_fix2(node),
_sub_gnss_heading(node),
@@ -90,6 +91,13 @@ UavcanGnssBridge::init()
return res;
}
res = _sub_quality.start(QualityCbBinder(this, &UavcanGnssBridge::gnss_quality_sub_cb));
if (res < 0) {
PX4_WARN("GNSS quality sub failed %i", res);
return res;
}
res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0) {
@@ -152,6 +160,13 @@ UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavc
_last_gnss_auxiliary_vdop = msg.vdop;
}
void
UavcanGnssBridge::gnss_quality_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Quality> &msg)
{
_last_gnss_quality_timestamp = hrt_absolute_time();
_last_quality = msg;
}
void
UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
@@ -557,6 +572,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
sensor_gps.rtcm_injection_rate = _rtcm_injection_rate;
// Apply cached Quality message fields (if received within last 2s)
if (hrt_elapsed_time(&_last_gnss_quality_timestamp) < 2_s) {
sensor_gps.noise_per_ms = _last_quality.noise;
sensor_gps.automatic_gain_control = _last_quality.agc;
sensor_gps.jamming_state = _last_quality.jamming_state;
sensor_gps.jamming_indicator = _last_quality.jamming_indicator;
sensor_gps.spoofing_state = _last_quality.spoofing_state;
sensor_gps.authentication_state = _last_quality.auth_state;
sensor_gps.diff_age = _last_quality.diff_age;
sensor_gps.antenna_status = _last_quality.antenna_status;
sensor_gps.antenna_power = _last_quality.antenna_power;
sensor_gps.fix_quality = _last_quality.fix_quality;
sensor_gps.system_error = _last_quality.system_errors;
}
publish(msg.getSrcNodeID().get(), &sensor_gps);
}
src/drivers/uavcan/sensors/gnss.hpp
+10
View File
@@ -55,6 +55,7 @@
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Quality.hpp>
#include <ardupilot/gnss/MovingBaselineData.hpp>
#include <ardupilot/gnss/RelPosHeading.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp>
@@ -82,6 +83,7 @@ private:
* GNSS fix message will be reported via this callback.
*/
void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
void gnss_quality_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Quality> &msg);
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg);
@@ -106,6 +108,10 @@ private:
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
AuxiliaryCbBinder;
typedef uavcan::MethodBinder < UavcanGnssBridge *,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Quality> &) >
QualityCbBinder;
typedef uavcan::MethodBinder < UavcanGnssBridge *,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
FixCbBinder;
@@ -129,6 +135,7 @@ private:
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
uavcan::Subscriber<uavcan::equipment::gnss::Quality, QualityCbBinder> _sub_quality;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCbBinder> _sub_gnss_heading;
@@ -143,6 +150,9 @@ private:
float _last_gnss_auxiliary_hdop{0.0f};
float _last_gnss_auxiliary_vdop{0.0f};
uint64_t _last_gnss_quality_timestamp{0};
uavcan::equipment::gnss::Quality _last_quality{};
uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection
uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections