Arm through external control

* Prepare for external control enabled flag gating ability to arm in DDS Signed-off-by: Ryan Friedman <[email protected]>
Ryanf55 · Nov 13, 2024 · 42bdaad · 42bdaad
1 parent f08a212
commit 42bdaad
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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 @@ 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
@@ -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
@@ -8,6 +8,7 @@
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 
+#include <AP_Arming/AP_Arming.h>
 #include <AP_Common/Location.h>
 #include <AP_Math/AP_Math.h>
 
@@ -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;
     }