Dds mode switch through external control

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -896,7 +896,11 @@ 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);
+#if AP_EXTERNAL_CONTROL_ENABLED
+        mode_switch_response.status = AP_DDS_External_Control::set_mode(mode_switch_request.mode);
+#else // AP_EXTERNAL_CONTROL_ENABLED
+        mode_switch_response.status = false;
+#endif // AP_EXTERNAL_CONTROL_ENABLED
         mode_switch_response.curr_mode = AP::vehicle()->get_mode();
 
         const uxrObjectId replier_id = {

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -4,9 +4,11 @@
 
 #include "AP_DDS_ExternalControl.h"
 #include "AP_DDS_Frames.h"
-#include <AP_AHRS/AP_AHRS.h>
 
+#include <AP_AHRS/AP_AHRS.h>
 #include <AP_ExternalControl/AP_ExternalControl.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Vehicle/ModeReason.h>
 
 // These are the Goal Interface constants. Because microxrceddsgen does not expose
 // them in the generated code, they are manually maintained
@@ -105,6 +107,16 @@ bool AP_DDS_External_Control::disarm(AP_Arming::Method method, bool do_disarm_ch
     return external_control->disarm(method, do_disarm_checks);
 }
 
+bool AP_DDS_External_Control::set_mode(const uint8_t mode)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    return external_control->set_mode(mode, ModeReason::DDS_COMMAND);
+}
+
 bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)
 {
 

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -15,6 +15,7 @@ class AP_DDS_External_Control
     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);
+    static bool set_mode(const uint8_t mode);
 
 private:
     static bool convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out);

libraries/AP_ExternalControl/AP_ExternalControl.cpp

@@ -2,6 +2,8 @@
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 
+#include <AP_Vehicle/AP_Vehicle.h>
+
 // singleton instance
 AP_ExternalControl *AP_ExternalControl::singleton;
 
@@ -15,6 +17,11 @@ bool AP_ExternalControl::disarm(AP_Arming::Method method, bool do_disarm_checks)
     return AP::arming().disarm(method, do_disarm_checks);
 }
 
+bool AP_ExternalControl::set_mode(const uint8_t mode, const ModeReason reason)
+{
+    return AP::vehicle()->set_mode(mode, reason);
+}
+
 AP_ExternalControl::AP_ExternalControl()
 {
     singleton = this;

libraries/AP_ExternalControl/AP_ExternalControl.h

@@ -11,6 +11,7 @@
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Common/Location.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Vehicle/ModeReason.h>
 
 class AP_ExternalControl
 {
@@ -43,6 +44,11 @@ class AP_ExternalControl
     */
     virtual bool disarm(AP_Arming::Method method, bool do_disarm_checks) WARN_IF_UNUSED;
 
+    /*
+        Set the mode of the vehicle. The mode number is vehicle specific.
+    */
+    virtual bool set_mode(const uint8_t mode, const ModeReason reason) WARN_IF_UNUSED;
+
     static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED {
         return singleton;
     }