From bb6c87213863f15ee56d029029d4677d9b1f038e Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 17 Apr 2023 17:31:28 -0600 Subject: [PATCH] AP_DDS: Publish NavSatFix as soon as its available * Removes the hard coded timing, now it's driven by the GPS update times * Changed the function signature to return true if the data topic has been changed Signed-off-by: Ryan Friedman --- libraries/AP_DDS/AP_DDS_Client.cpp | 44 ++++++++++++++++++------------ libraries/AP_DDS/AP_DDS_Client.h | 2 +- 2 files changed, 28 insertions(+), 18 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 94c8213f65..15631bd20a 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -13,7 +13,6 @@ #include "AP_DDS_Client.h" static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; -static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000; static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; static char WGS_84_FRAME_ID[] = "WGS-84"; // https://www.ros.org/reps/rep-0105.html#base-link @@ -41,7 +40,7 @@ void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) } -void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) +bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) { // Add a lambda that takes in navsatfix msg and populates the cov // Make it constexpr if possible @@ -50,14 +49,9 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i // assert(instance >= GPS_MAX_RECEIVERS); if (instance >= GPS_MAX_RECEIVERS) { - return; + return false; } - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, WGS_84_FRAME_ID); - msg.status.service = 0; // SERVICE_GPS - msg.status.status = -1; // STATUS_NO_FIX - auto &gps = AP::gps(); WITH_SEMAPHORE(gps.get_semaphore()); @@ -65,9 +59,23 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i msg.status.status = -1; // STATUS_NO_FIX msg.status.service = 0; // No services supported msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return; + return false; } + // No update is needed + const auto last_fix_time_ms = gps.last_fix_time_ms(instance); + if (last_nav_sat_fix_time_ms == last_fix_time_ms) { + return false; + } else { + last_nav_sat_fix_time_ms = last_fix_time_ms; + } + + + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id, WGS_84_FRAME_ID); + msg.status.service = 0; // SERVICE_GPS + msg.status.status = -1; // STATUS_NO_FIX + //! @todo What about glonass, compass, galileo? //! This will be properly designed and implemented to spec in #23277 @@ -79,7 +87,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i case AP_GPS::NO_FIX: msg.status.status = -1; // STATUS_NO_FIX msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return; + return true; case AP_GPS::GPS_OK_FIX_2D: case AP_GPS::GPS_OK_FIX_3D: msg.status.status = 0; // STATUS_FIX @@ -104,7 +112,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i // With absolute frame, this condition is unlikely msg.status.status = -1; // STATUS_NO_FIX msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return; + return true; } msg.altitude = alt_cm / 100.0; @@ -120,6 +128,8 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i msg.position_covariance[6] = cov[2][0]; msg.position_covariance[7] = cov[2][1]; msg.position_covariance[8] = cov[2][2]; + + return true; } void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) @@ -438,16 +448,16 @@ void AP_DDS_Client::update() write_time_topic(); } - if (cur_time_ms - last_nav_sat_fix_time_ms > DELAY_NAV_SAT_FIX_TOPIC_MS) { - constexpr uint8_t instance = 0; - update_topic(nav_sat_fix_topic, instance); - last_nav_sat_fix_time_ms = cur_time_ms; + constexpr uint8_t gps_instance = 0; + if (update_topic(nav_sat_fix_topic, gps_instance)) { write_nav_sat_fix_topic(); } + + if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { - constexpr uint8_t instance = 0; - update_topic(battery_state_topic, instance); + constexpr uint8_t battery_instance = 0; + update_topic(battery_state_topic, battery_instance); last_battery_state_time_ms = cur_time_ms; write_battery_state_topic(); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 948bce659f..37985ed20d 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -53,7 +53,7 @@ private: bool connected = true; static void update_topic(builtin_interfaces_msg_Time& msg); - static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance); + bool update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) WARN_IF_UNUSED; static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance);