aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DDS: use generated types for service serialisation

Browse Source 
Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
Rhys Mainwaring 2023-09-29 21:01:45 +01:00 committed by Andrew Tridgell
parent 091c315fa4
commit fbdd4d65c5
1 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -12,6 +12,10 @@
#include <AP_Arming/AP_Arming.h> #include <AP_Arming/AP_Arming.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h> #include <AP_ExternalControl/AP_ExternalControl_config.h>
#include "ardupilot_msgs/srv/ArmMotors.h"
#include "ardupilot_msgs/srv/ModeSwitch.h"
#if AP_EXTERNAL_CONTROL_ENABLED #if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h" #include "AP_DDS_ExternalControl.h"
#endif #endif
@ -501,19 +505,19 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
(void) length; (void) length;
switch (object_id.id) { switch (object_id.id) {
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: { case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
bool arm; ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
bool result; ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
const bool deserialize_success = ucdr_deserialize_bool(ub,&arm); const bool deserialize_success = ardupilot_msgs_srv_ArmMotors_Request_deserialize_topic(ub, &arm_motors_request);
if (deserialize_success == false) { if (deserialize_success == false) {
break; break;
} }
if (arm) { if (arm_motors_request.arm) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for arming received"); GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for arming received");
result = AP::arming().arm(AP_Arming::Method::DDS); arm_motors_response.result = AP::arming().arm(AP_Arming::Method::DDS);
} else { } else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for disarming received"); GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for disarming received");
result = AP::arming().disarm(AP_Arming::Method::DDS); arm_motors_response.result = AP::arming().disarm(AP_Arming::Method::DDS);
} }
const uxrObjectId replier_id = { const uxrObjectId replier_id = {
@ -521,55 +525,52 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
.type = UXR_REPLIER_ID .type = UXR_REPLIER_ID
}; };
//Todo : Fix the size-handling of services with the help of the functions autogenerated via Micro-XRCE-DDS Gen
uint8_t reply_buffer[8] {}; uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub; ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
const bool serialize_success = ucdr_serialize_bool(&reply_ub,result); const bool serialize_success = ardupilot_msgs_srv_ArmMotors_Response_serialize_topic(&reply_ub, &arm_motors_response);
if (serialize_success == false) { if (serialize_success == false) {
break; break;
} }
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (result) { if (arm_motors_response.result) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : SUCCESS"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Arming/Disarming : SUCCESS");
} else { } else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : FAIL"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Arming/Disarming : FAIL");
} }
break; break;
} }
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: { case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
uint8_t mode; ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
const bool deserialize_success = ucdr_deserialize_uint8_t(ub,&mode); ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response;
const bool deserialize_success = ardupilot_msgs_srv_ModeSwitch_Request_deserialize_topic(ub, &mode_switch_request);
if (deserialize_success == false) { if (deserialize_success == false) {
break; break;
} }
bool status = AP::vehicle()->set_mode(mode, ModeReason::DDS_COMMAND); mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
uint8_t curr_mode = AP::vehicle()->get_mode(); mode_switch_response.curr_mode = AP::vehicle()->get_mode();
const uxrObjectId replier_id = { const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id, .id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id,
.type = UXR_REPLIER_ID .type = UXR_REPLIER_ID
}; };
//Todo : Fix the size-handling of services with the help of the functions autogenerated via Micro-XRCE-DDS Gen
uint8_t reply_buffer[8] {}; uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub; ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
bool serialize_success = true; const bool serialize_success = ardupilot_msgs_srv_ModeSwitch_Response_serialize_topic(&reply_ub, &mode_switch_response);
serialize_success &= ucdr_serialize_bool(&reply_ub, status); if (serialize_success == false) {
serialize_success &= ucdr_serialize_uint8_t(&reply_ub, curr_mode);
if (serialize_success == false || reply_ub.error) {
break; break;
} }
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (status) { if (mode_switch_response.status) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : SUCCESS"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Mode Switch : SUCCESS");
} else { } else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : FAIL"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Mode Switch : FAIL");
} }
break; break;
} }
@ -839,6 +840,7 @@ void AP_DDS_Client::write_battery_state_topic()
} }
} }
} }
void AP_DDS_Client::write_local_pose_topic() void AP_DDS_Client::write_local_pose_topic()
{ {
WITH_SEMAPHORE(csem); WITH_SEMAPHORE(csem);