diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 0a7a81689b802..4cd105188b6d8 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/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 = { diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index 259dcb51601c9..fdc8244be0fa7 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -4,9 +4,11 @@ #include "AP_DDS_ExternalControl.h" #include "AP_DDS_Frames.h" -#include +#include #include +#include +#include // 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) { diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index eeb86e5e9c132..5c7c15c3c9131 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/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); diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.cpp b/libraries/AP_ExternalControl/AP_ExternalControl.cpp index 92a97f18b7572..968940248c13e 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.cpp +++ b/libraries/AP_ExternalControl/AP_ExternalControl.cpp @@ -2,6 +2,8 @@ #if AP_EXTERNAL_CONTROL_ENABLED +#include + // 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; diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index 57670008f4896..df9b1256e48ab 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -11,6 +11,7 @@ #include #include #include +#include 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; }