Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues 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_Vehicle/AP_Vehicle.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
#include "AP_DDS_ExternalControl.h"
#endif
@ -501,19 +505,19 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
(void) length;
switch (object_id.id) {
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
bool arm;
bool result;
const bool deserialize_success = ucdr_deserialize_bool(ub,&arm);
ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
const bool deserialize_success = ardupilot_msgs_srv_ArmMotors_Request_deserialize_topic(ub, &arm_motors_request);
if (deserialize_success == false) {
break;
}
if (arm) {
if (arm_motors_request.arm) {
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 {
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 = {
@ -521,55 +525,52 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
.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] {};
ucdrBuffer reply_ub;
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) {
break;
}
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");
if (arm_motors_response.result) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Arming/Disarming : SUCCESS");
} 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;
}
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
uint8_t mode;
const bool deserialize_success = ucdr_deserialize_uint8_t(ub,&mode);
ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
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) {
break;
}
bool status = AP::vehicle()->set_mode(mode, ModeReason::DDS_COMMAND);
uint8_t curr_mode = AP::vehicle()->get_mode();
mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
mode_switch_response.curr_mode = AP::vehicle()->get_mode();
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_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] {};
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
bool serialize_success = true;
serialize_success &= ucdr_serialize_bool(&reply_ub, status);
serialize_success &= ucdr_serialize_uint8_t(&reply_ub, curr_mode);
if (serialize_success == false || reply_ub.error) {
const bool serialize_success = ardupilot_msgs_srv_ModeSwitch_Response_serialize_topic(&reply_ub, &mode_switch_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (status) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : SUCCESS");
if (mode_switch_response.status) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Mode Switch : SUCCESS");
} 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;
}
@ -839,6 +840,7 @@ void AP_DDS_Client::write_battery_state_topic()
}
}
}
void AP_DDS_Client::write_local_pose_topic()
{
WITH_SEMAPHORE(csem);