mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-10 06:39:25 +08:00
uavcan: align gnss.quality with sensor_gnss_status / GNSS_INTEGRITY
EKF Update Change Indicator / unit_tests (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Drop fix_quality (u8, 0-100) from SensorGps.msg and route the four abstract quality metrics (corrections, receiver summary, gnss signal, post-processing) through sensor_gnss_status (u8, 0-10, 255 = N/A) — matching MAVLink GNSS_INTEGRITY (id 441). - Bridge publishes sensor_gnss_status on incoming Quality messages. - uavcannode GnssQuality publisher reads the four quality fields from sensor_gnss_status while continuing to trigger on sensor_gps. - Submodule bumps for DSDL and PX4-GPSDrivers.
This commit is contained in:
@@ -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])
|
||||
|
||||
+1
-1
Submodule src/drivers/gps/devices updated: 99427567b6...51103d82ac
Submodule src/drivers/uavcan/libdronecan/dsdl updated: 6035657d00...9ef8622436
@@ -583,8 +583,17 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
||||
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);
|
||||
|
||||
@@ -48,6 +48,7 @@
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_gnss_status.h>
|
||||
#include <uORB/topics/gps_inject_data.h>
|
||||
#include <uORB/topics/gps_dump.h>
|
||||
|
||||
@@ -153,6 +154,8 @@ private:
|
||||
uint64_t _last_gnss_quality_timestamp{0};
|
||||
uavcan::equipment::gnss::Quality _last_quality{};
|
||||
|
||||
uORB::PublicationMulti<sensor_gnss_status_s> _sensor_gnss_status_pub{ORB_ID(sensor_gnss_status)};
|
||||
|
||||
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
|
||||
|
||||
@@ -37,8 +37,10 @@
|
||||
|
||||
#include <uavcan/equipment/gnss/Quality.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_gnss_status.h>
|
||||
|
||||
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<uavcan::equipment::gnss::Quality>::broadcast(quality);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
uORB::Subscription _sensor_gnss_status_sub{ORB_ID(sensor_gnss_status)};
|
||||
};
|
||||
} // namespace uavcannode
|
||||
|
||||
Reference in New Issue
Block a user