diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 05ba28f06dc7a..12f8b46ad52e6 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -815,7 +815,12 @@ 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 + arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS); + 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, diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index 490ce519defe9..5e78773bd2ed4 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -85,6 +85,24 @@ 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) { diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index 3986ef63774aa..ad9a266dbefd5 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h @@ -3,7 +3,7 @@ #if AP_DDS_ENABLED #include "ardupilot_msgs/msg/GlobalPosition.h" #include "geometry_msgs/msg/TwistStamped.h" - +#include #include class AP_DDS_External_Control @@ -13,6 +13,9 @@ class AP_DDS_External_Control // 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=true); + static bool disarm(AP_Arming::Method method, bool do_disarm_checks=true); + private: static bool convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out); }; diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.cpp b/libraries/AP_ExternalControl/AP_ExternalControl.cpp index e0e92f0a29911..7fd8b4da5b371 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.cpp +++ b/libraries/AP_ExternalControl/AP_ExternalControl.cpp @@ -5,6 +5,14 @@ // singleton instance AP_ExternalControl *AP_ExternalControl::singleton; +bool AP_ExternalControl::arm(AP_Arming::Method method, bool do_arming_checks) { + return AP::arming().arm(method, do_arming_checks); +} + +bool AP_ExternalControl::disarm(AP_Arming::Method method, bool do_disarm_checks) { + return AP::arming().disarm(method, do_disarm_checks); +} + AP_ExternalControl::AP_ExternalControl() { singleton = this; diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index 34228e2b7ab4f..fd417d02764c7 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -8,6 +8,7 @@ #if AP_EXTERNAL_CONTROL_ENABLED +#include #include #include @@ -32,6 +33,16 @@ class AP_ExternalControl return false; } + /* + Arm the vehicle + */ + virtual bool arm(AP_Arming::Method method, bool do_arming_checks=true); + + /* + Disarm the vehicle + */ + virtual bool disarm(AP_Arming::Method method, bool do_disarm_checks=true); + static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED { return singleton; }