Skip to content

Commit

Permalink
AP_DDS: Added verbose debugging output messages [skip ci]
Browse files Browse the repository at this point in the history
  • Loading branch information
@arshPratap
arshPratap committed Aug 24, 2023
1 parent 5e6591f commit 7d269e0
Show file tree
Hide file tree
Showing 2 changed files with 44 additions and 18 deletions.
59 changes: 41 additions & 18 deletions libraries/AP_DDS/AP_DDS_Client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,14 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {

#endif

// @Param: _DEBUG
// @DisplayName: DDS debug level
// @Description: When set to 1, this enables the verbose debugging output for DDS. This can be used to diagnose failures.
// @Values: 0:Disabled,1:Debug (basic debug output) ,2:Debug Errors (display errors), 3:Debug Error Status code
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO("_DEBUG", 3, AP_DDS_Client, debug_level, 0),

AP_GROUPEND
};

Expand Down Expand Up @@ -445,11 +453,11 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin

subscribe_sample_count++;
if (rx_joy_topic.axes_size >= 4) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
debug("Received sensor_msgs/Joy: %f, %f, %f, %f",
rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
// TODO implement joystick RC control to AP
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size ");
debug("Received sensor_msgs/Joy: Insufficient axes size ");
}
break;
}
Expand All @@ -465,8 +473,11 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
#endif // AP_DDS_VISUALODOM_ENABLED

debug("Received tf2_msgs/TFMessage of length: %u",
static_cast<unsigned>(rx_dynamic_transforms_topic.transforms_size));
// TODO implement external odometry to AP
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage: Insufficient size ");
debug("Received tf2_msgs/TFMessage: Insufficient size ");
}
break;
}
Expand Down Expand Up @@ -511,10 +522,10 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
}

if (arm) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for arming received");
debug("Request for arming received");
result = AP::arming().arm(AP_Arming::Method::DDS);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for disarming received");
debug("Request for disarming received");
result = AP::arming().disarm(AP_Arming::Method::DDS);
}

Expand All @@ -537,9 +548,9 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u

uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (result) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : SUCCESS");
debug("Request for Arming/Disarming : SUCCESS");
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : FAIL");
debug("Request for Arming/Disarming : FAIL");
}
break;
}
Expand Down Expand Up @@ -674,14 +685,18 @@ bool AP_DDS_Client::create()
requests[2] = dwriter_req_id;

if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Pub/Writer session request failure for index '%u'",i);
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
//session request failure displayed on debug level 2 and above
debug_error("XRCE Client: Topic/Pub/Writer session request failure for index '%u'",i);
if (debug_level > 2){
for (uint8_t s = 0 ; s < nRequests; s++) {
// status codes displayed on debug level 3 and above
debug_error("XRCE Client: Status '%d' result '%u'", s, status[s]);
}
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Pub/Writer session pass for index '%u'",i);
debug("XRCE Client: Topic/Pub/Writer session pass for index '%u'",i);
}
} else if (strlen(topics[i].dr_profile_label) > 0) {
// Subscriber
Expand All @@ -702,14 +717,18 @@ bool AP_DDS_Client::create()
requests[2] = dreader_req_id;

if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Sub/Reader session request failure for index '%u'",i);
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
// session request failure displayed on debug_level 2 and above
debug_error("XRCE Client: Topic/Sub/Reader session request failure for index '%u'",i);
if(debug_level > 2){
for (uint8_t s = 0 ; s < nRequests; s++) {
// status codes displayed on debug level 3 and above
debug_error("XRCE Client: Status '%d' result '%u'", s, status[s]);
}
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Sub/Reader session pass for index '%u'",i);
debug("XRCE Client: Topic/Sub/Reader session pass for index '%u'",i);
uxr_buffer_request_data(&session, reliable_out, topics[i].dr_id, reliable_in, &delivery_control);
}
}
Expand All @@ -733,12 +752,16 @@ bool AP_DDS_Client::create()
uint8_t status;

if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, &request, &status, 1)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Service/Replier session request failure for index '%u'",i);
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status result '%u'", status);
// session request failure displayed on debug level 2 and above
debug_error("XRCE Client: Service/Replier session request failure for index '%u'",i);
if(debug_level > 2){
// status codes displayed on debug level 3 and above
debug_error("XRCE Client: Status result '%u'", status);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Service/Replier session pass for index '%u'",i);
debug("XRCE Client: Service/Replier session pass for index '%u'",i);
uxr_buffer_request_data(&session, reliable_out, rep_id, reliable_in, &delivery_control);
}

Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_DDS/AP_DDS_Client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
#define DDS_MTU 512
#define DDS_STREAM_HISTORY 8
#define DDS_BUFFER_SIZE DDS_MTU * DDS_STREAM_HISTORY
#define debug(fmt, args ...) do { if (debug_level) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS_DEBUG: " fmt, ## args); } } while (0)
#define debug_error(fmt, args ...) do { if (debug_level > 1) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DDS_DEBUG_ERROR: " fmt, ## args); } } while (0)

#if AP_DDS_UDP_ENABLED
#include <AP_HAL/utility/Socket.h>
Expand All @@ -41,6 +43,7 @@ class AP_DDS_Client
private:

AP_Int8 enabled;
AP_Int8 debug_level;

// Serial Allocation
uxrSession session; //Session
Expand Down

0 comments on commit 7d269e0

Please sign in to comment.