diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 1708f68092..44cddba42e 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -87,8 +87,6 @@ uint8 ANTENNA_POWER_OFF = 1 uint8 ANTENNA_POWER_ON = 2 uint8 antenna_power # Antenna power status -uint8 fix_quality # Fix quality/confidence indicator [0-100]. 0 = no confidence, 100 = full confidence. - float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 99427567b6..51103d82ac 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 99427567b66a4ddd01384ed24097014a02acc70f +Subproject commit 51103d82ac2697a78bc028c9fe957f2d23f9b66a diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl index 6035657d00..9ef8622436 160000 --- a/src/drivers/uavcan/libdronecan/dsdl +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -1 +1 @@ -Subproject commit 6035657d000b4d7f414243d3a9b15f71ae634631 +Subproject commit 9ef86224366d63de32502bc1ed1ecb9b76f2f596 diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 609306f7d3..bdd812fa9d 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -583,8 +583,17 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure 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; + + sensor_gnss_status_s sensor_gnss_status{}; + sensor_gnss_status.timestamp = hrt_absolute_time(); + sensor_gnss_status.device_id = sensor_gps.device_id; + sensor_gnss_status.quality_available = true; + sensor_gnss_status.quality_corrections = _last_quality.corrections_quality; + sensor_gnss_status.quality_receiver = _last_quality.system_status_summary; + sensor_gnss_status.quality_gnss_signals = _last_quality.gnss_signal_quality; + sensor_gnss_status.quality_post_processing = _last_quality.post_processing_quality; + _sensor_gnss_status_pub.publish(sensor_gnss_status); } publish(msg.getSrcNodeID().get(), &sensor_gps); diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index cac5b5215b..118823ccbc 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -153,6 +154,8 @@ private: uint64_t _last_gnss_quality_timestamp{0}; uavcan::equipment::gnss::Quality _last_quality{}; + uORB::PublicationMulti _sensor_gnss_status_pub{ORB_ID(sensor_gnss_status)}; + 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 diff --git a/src/drivers/uavcannode/Publishers/GnssQuality.hpp b/src/drivers/uavcannode/Publishers/GnssQuality.hpp index d9f530b04e..8ce9f78f84 100644 --- a/src/drivers/uavcannode/Publishers/GnssQuality.hpp +++ b/src/drivers/uavcannode/Publishers/GnssQuality.hpp @@ -37,8 +37,10 @@ #include +#include #include #include +#include namespace uavcannode { @@ -71,7 +73,6 @@ public: { using uavcan::equipment::gnss::Quality; - // sensor_gps -> uavcan::equipment::gnss::Quality sensor_gps_s gps; if (uORB::SubscriptionCallbackWorkItem::update(&gps)) { @@ -86,14 +87,29 @@ public: quality.diff_age = gps.diff_age; quality.antenna_status = gps.antenna_status; quality.antenna_power = gps.antenna_power; - quality.fix_quality = gps.fix_quality; quality.system_errors = gps.system_error; + quality.corrections_quality = 255; + quality.system_status_summary = 255; + quality.gnss_signal_quality = 255; + quality.post_processing_quality = 255; + + sensor_gnss_status_s status; + + if (_sensor_gnss_status_sub.copy(&status) && status.quality_available) { + quality.corrections_quality = status.quality_corrections; + quality.system_status_summary = status.quality_receiver; + quality.gnss_signal_quality = status.quality_gnss_signals; + quality.post_processing_quality = status.quality_post_processing; + } + uavcan::Publisher::broadcast(quality); - // ensure callback is registered uORB::SubscriptionCallbackWorkItem::registerCallback(); } } + +private: + uORB::Subscription _sensor_gnss_status_sub{ORB_ID(sensor_gnss_status)}; }; } // namespace uavcannode