mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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 <ryanfriedman5410+github@gmail.com>
This commit is contained in:
parent
edde5d2e48
commit
bb6c872138
@ -13,7 +13,6 @@
|
|||||||
#include "AP_DDS_Client.h"
|
#include "AP_DDS_Client.h"
|
||||||
|
|
||||||
static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
|
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 constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
|
||||||
static char WGS_84_FRAME_ID[] = "WGS-84";
|
static char WGS_84_FRAME_ID[] = "WGS-84";
|
||||||
// https://www.ros.org/reps/rep-0105.html#base-link
|
// 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
|
// Add a lambda that takes in navsatfix msg and populates the cov
|
||||||
// Make it constexpr if possible
|
// 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);
|
// assert(instance >= GPS_MAX_RECEIVERS);
|
||||||
if (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();
|
auto &gps = AP::gps();
|
||||||
WITH_SEMAPHORE(gps.get_semaphore());
|
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.status = -1; // STATUS_NO_FIX
|
||||||
msg.status.service = 0; // No services supported
|
msg.status.service = 0; // No services supported
|
||||||
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
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?
|
//! @todo What about glonass, compass, galileo?
|
||||||
//! This will be properly designed and implemented to spec in #23277
|
//! 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:
|
case AP_GPS::NO_FIX:
|
||||||
msg.status.status = -1; // STATUS_NO_FIX
|
msg.status.status = -1; // STATUS_NO_FIX
|
||||||
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
||||||
return;
|
return true;
|
||||||
case AP_GPS::GPS_OK_FIX_2D:
|
case AP_GPS::GPS_OK_FIX_2D:
|
||||||
case AP_GPS::GPS_OK_FIX_3D:
|
case AP_GPS::GPS_OK_FIX_3D:
|
||||||
msg.status.status = 0; // STATUS_FIX
|
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
|
// With absolute frame, this condition is unlikely
|
||||||
msg.status.status = -1; // STATUS_NO_FIX
|
msg.status.status = -1; // STATUS_NO_FIX
|
||||||
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
||||||
return;
|
return true;
|
||||||
}
|
}
|
||||||
msg.altitude = alt_cm / 100.0;
|
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[6] = cov[2][0];
|
||||||
msg.position_covariance[7] = cov[2][1];
|
msg.position_covariance[7] = cov[2][1];
|
||||||
msg.position_covariance[8] = cov[2][2];
|
msg.position_covariance[8] = cov[2][2];
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
|
void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
|
||||||
@ -438,16 +448,16 @@ void AP_DDS_Client::update()
|
|||||||
write_time_topic();
|
write_time_topic();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (cur_time_ms - last_nav_sat_fix_time_ms > DELAY_NAV_SAT_FIX_TOPIC_MS) {
|
constexpr uint8_t gps_instance = 0;
|
||||||
constexpr uint8_t instance = 0;
|
if (update_topic(nav_sat_fix_topic, gps_instance)) {
|
||||||
update_topic(nav_sat_fix_topic, instance);
|
|
||||||
last_nav_sat_fix_time_ms = cur_time_ms;
|
|
||||||
write_nav_sat_fix_topic();
|
write_nav_sat_fix_topic();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
|
if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
|
||||||
constexpr uint8_t instance = 0;
|
constexpr uint8_t battery_instance = 0;
|
||||||
update_topic(battery_state_topic, instance);
|
update_topic(battery_state_topic, battery_instance);
|
||||||
last_battery_state_time_ms = cur_time_ms;
|
last_battery_state_time_ms = cur_time_ms;
|
||||||
write_battery_state_topic();
|
write_battery_state_topic();
|
||||||
}
|
}
|
||||||
|
@ -53,7 +53,7 @@ private:
|
|||||||
bool connected = true;
|
bool connected = true;
|
||||||
|
|
||||||
static void update_topic(builtin_interfaces_msg_Time& msg);
|
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 populate_static_transforms(tf2_msgs_msg_TFMessage& msg);
|
||||||
static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance);
|
static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance);
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user