From 1ac2319bf2b5019eb962d0a05f845007ec595286 Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Fri, 18 Oct 2024 21:39:44 +0000 Subject: [PATCH] AP_DDS: External Control enable --- libraries/AP_DDS/AP_DDS_Client.cpp | 30 +++++++++++++++---- libraries/AP_DDS/AP_DDS_config.h | 4 +-- .../AP_ExternalControl/AP_ExternalControl.h | 21 +++++++++++++ libraries/RC_Channel/RC_Channel.cpp | 21 +++++++++++++ libraries/RC_Channel/RC_Channel.h | 1 + 5 files changed, 70 insertions(+), 7 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 36d8d1bf714f6..67f83441543c2 100644 --- 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 #include +#include #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 index b8f31e368c8a4..034e3f0d78ab3 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -3,8 +3,8 @@ #include #include -#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 index 34228e2b7ab4f..10af1eb71435b 100644 --- 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 index 4b4f30d79e288..8834d83fa3a69 100644 --- 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 #include #include +#include +#include #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 index 1ad2e284a6ad4..59195030ebdaf 100644 --- 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