diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl index 993be80a62..6035657d00 160000 --- a/src/drivers/uavcan/libdronecan/dsdl +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -1 +1 @@ -Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 +Subproject commit 6035657d000b4d7f414243d3a9b15f71ae634631 diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index fb02f1d274..609306f7d3 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -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 &msg) +{ + _last_gnss_quality_timestamp = hrt_absolute_time(); + _last_quality = msg; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { @@ -557,6 +572,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure 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); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 72fe9b3453..cac5b5215b 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ private: * GNSS fix message will be reported via this callback. */ void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); + void gnss_quality_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -106,6 +108,10 @@ private: void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > AuxiliaryCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + QualityCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > FixCbBinder; @@ -129,6 +135,7 @@ private: uavcan::INode &_node; uavcan::Subscriber _sub_auxiliary; + uavcan::Subscriber _sub_quality; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; uavcan::Subscriber _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 _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