Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DDS: arm through external control 

* Prepare for external control enabled flag gating ability to arm in DDS

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
This commit is contained in:
Ryan Friedman 2024-11-13 11:46:21 -08:00 committed by Peter Barker
parent 0953c9dda3
commit 489e8473a2
3 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -817,7 +817,13 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for %sing received", msg_prefix, arm_motors_request.arm ? "arm" : "disarm");
arm_motors_response.result = arm_motors_request.arm ? AP::arming().arm(AP_Arming::Method::DDS) : AP::arming().disarm(AP_Arming::Method::DDS);
#if AP_EXTERNAL_CONTROL_ENABLED
const bool do_checks = true;
arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS, do_checks) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS, do_checks);
if (!arm_motors_response.result) {
// TODO #23430 handle arm failure through rosout, throttled.
}
#endif // AP_EXTERNAL_CONTROL_ENABLED
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,

20
libraries/AP_DDS/AP_DDS_ExternalControl.cpp
View File

@ -85,6 +85,26 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
return false;
}
bool AP_DDS_External_Control::arm(AP_Arming::Method method, bool do_arming_checks)
{
auto *external_control = AP::externalcontrol();
if (external_control == nullptr) {
return false;
}
return external_control->arm(method, do_arming_checks);
}
bool AP_DDS_External_Control::disarm(AP_Arming::Method method, bool do_disarm_checks)
{
auto *external_control = AP::externalcontrol();
if (external_control == nullptr) {
return false;
}
return external_control->disarm(method, do_disarm_checks);
}
bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out)
{

5
libraries/AP_DDS/AP_DDS_ExternalControl.h
View File

@ -3,7 +3,7 @@
#if AP_DDS_ENABLED
#include "ardupilot_msgs/msg/GlobalPosition.h"
#include "geometry_msgs/msg/TwistStamped.h"
#include <AP_Arming/AP_Arming.h>
#include <AP_Common/Location.h>
class AP_DDS_External_Control
@ -13,6 +13,9 @@ public:
// https://ros.org/reps/rep-0147.html#goal-interface
static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
static bool arm(AP_Arming::Method method, bool do_arming_checks);
static bool disarm(AP_Arming::Method method, bool do_disarm_checks);
private:
static bool convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out);
};