AP_DDS: External Control enable

ArduPilot · Nov 5, 2024 · 1ac2319 · 1ac2319
1 parent 75af2d8
commit 1ac2319
Show file tree

Hide file tree

Showing 5 changed files with 70 additions and 7 deletions.
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -18,6 +18,7 @@
 # endif // AP_DDS_ARM_SERVER_ENABLED
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
+#include <AP_ExternalControl/AP_ExternalControl.h>
 
 #if AP_DDS_ARM_SERVER_ENABLED
 #include "ardupilot_msgs/srv/ArmMotors.h"
@@ -644,6 +645,13 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
     (void) request_id;
     (void) stream_id;
     (void) length;
+    auto *external_control = AP::externalcontrol();
+    // TODO: Replace this line before merging. I leave the Joystick control always enable to test the RC functionality.
+    if (!external_control->is_enabled()) {
+    // if (!external_control->is_enabled() && object_id.id != topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Command rejected: External Control Disabled", msg_prefix);
+        return;
+    }
     switch (object_id.id) {
 #if AP_DDS_JOY_SUB_ENABLED
     case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: {
@@ -739,6 +747,8 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
 {
     (void) request_id;
     (void) length;
+    // Verify if external control is enabled.
+    auto *external_control = AP::externalcontrol();
     switch (object_id.id) {
 #if AP_DDS_ARM_SERVER_ENABLED
     case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
@@ -749,8 +759,13 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
             break;
         }
 
-        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 (external_control->is_enabled()) {
+            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);
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Arming Request rejected: External Control Disabled", msg_prefix);
+            arm_motors_response.result = false;
+        }
 
         const uxrObjectId replier_id = {
             .id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,
@@ -779,9 +794,14 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         if (deserialize_success == false) {
             break;
         }
-        mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
-        mode_switch_response.curr_mode = AP::vehicle()->get_mode();
-
+        if (external_control->is_enabled()) {
+            mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
+            mode_switch_response.curr_mode = AP::vehicle()->get_mode();
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Mode Switch Request rejected: External Control Disabled", msg_prefix);
+            mode_switch_response.status = false;
+            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

diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h
@@ -3,8 +3,8 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_Networking/AP_Networking_Config.h>
 
-#ifndef AP_DDS_ENABLED
-#define AP_DDS_ENABLED 1
+#ifndef AP_EXTERNAL_CONTROL_ENABLED
+#define AP_EXTERNAL_CONTROL_ENABLED 1
 #endif
 
 // UDP only on SITL for now

diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h
@@ -32,6 +32,26 @@ class AP_ExternalControl
         return false;
     }
 
+    /*
+        Set the External control status.
+    */
+    void enable()
+    {
+        enabled = true;
+    }
+
+    void disable()
+    {
+        enabled = false;
+    }
+
+    /*
+        Get the External control status.
+    */
+    bool is_enabled() WARN_IF_UNUSED {
+        return enabled;
+    }
+
     static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED {
         return singleton;
     }
@@ -40,6 +60,7 @@ class AP_ExternalControl
 
 private:
     static AP_ExternalControl *singleton;
+    bool enabled {true};
 };
 
 

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
@@ -60,6 +60,8 @@ extern const AP_HAL::HAL& hal;
 #include <AP_Torqeedo/AP_Torqeedo.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_Parachute/AP_Parachute_config.h>
+#include <AP_ExternalControl/AP_ExternalControl.h>
+#include <AP_DDS/AP_DDS_config.h>
 #define SWITCH_DEBOUNCE_TIME_MS  200
 
 const AP_Param::GroupInfo RC_Channel::var_info[] = {
@@ -768,6 +770,9 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos
     case AUX_FUNC::CAMERA_AUTO_FOCUS:
     case AUX_FUNC::CAMERA_LENS:
 #endif
+#if AP_DDS_ENABLED
+    case AUX_FUNC::EXTERNAL_CONTROL:
+#endif
 #if AP_AHRS_ENABLED
     case AUX_FUNC::AHRS_TYPE:
         run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
@@ -1845,6 +1850,22 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch
     }
 #endif
 
+#if AP_EXTERNAL_CONTROL_ENABLED
+    case AUX_FUNC::EXTERNAL_CONTROL: {
+        AP_ExternalControl *external_control = AP_ExternalControl::get_singleton();
+        if (external_control != nullptr) {
+            if (ch_flag == AuxSwitchPos::HIGH) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RC: External Control Enabled");
+                external_control->enable();
+            } else {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RC: External Control Disabled");
+                external_control->disable();
+            }
+        }
+        break;        
+    }
+#endif // AP_EXTERNAL_CONTROL_ENABLED 
+
     // do nothing for these functions
 #if HAL_MOUNT_ENABLED
     case AUX_FUNC::MOUNT1_ROLL:

diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
@@ -260,6 +260,7 @@ class RC_Channel {
         FLIGHTMODE_PAUSE =   178,  // e.g. pause movement towards waypoint
         ICE_START_STOP =     179, // AP_ICEngine start stop
         AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains
+        EXTERNAL_CONTROL = 181, // Enable/Disable external control from DDS interface
 
 
         // inputs from 200 will eventually used to replace RCMAP