Skip to content

Commit

Permalink
AP_DDS: RC Created option to trigger External Control
Browse files Browse the repository at this point in the history
  • Loading branch information
@tizianofiorenzani
tizianofiorenzani committed Oct 18, 2024
1 parent 59e102f commit f46bde5
Show file tree
Hide file tree
Showing 4 changed files with 63 additions and 10 deletions.
19 changes: 19 additions & 0 deletions ArduCopter/RC_Channel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,9 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
case AUX_FUNC::FORCEFLYING:
case AUX_FUNC::CUSTOM_CONTROLLER:
case AUX_FUNC::WEATHER_VANE_ENABLE:
#if AP_DDS_ENABLED
case AUX_FUNC::DDS_EXTERNAL_CONTROL:
#endif
case AUX_FUNC::TRANSMITTER_TUNING:
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
break;
Expand Down Expand Up @@ -667,6 +670,22 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
// do nothing, used in tuning.cpp for transmitter based tuning
break;

#if AP_DDS_ENABLED
case AUX_FUNC::DDS_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_DDS_ENABLED

default:
return RC_Channel::do_aux_function(ch_option, ch_flag);
}
Expand Down
19 changes: 19 additions & 0 deletions ArduPlane/RC_Channel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
case AUX_FUNC::ARSPD_CALIBRATE:
#endif
case AUX_FUNC::TER_DISABLE:
#if AP_DDS_ENABLED
case AUX_FUNC::DDS_EXTERNAL_CONTROL:
#endif
case AUX_FUNC::CROW_SELECT:
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
break;
Expand Down Expand Up @@ -458,6 +461,22 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
break;
#endif

#if AP_DDS_ENABLED
case AUX_FUNC::DDS_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_DDS_ENABLED

default:
return RC_Channel::do_aux_function(ch_option, ch_flag);
}
Expand Down
28 changes: 19 additions & 9 deletions libraries/AP_DDS/AP_DDS_Client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -638,7 +638,9 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
(void) stream_id;
(void) length;
auto *external_control = AP::externalcontrol();
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) {
Expand Down Expand Up @@ -736,10 +738,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();
if (!external_control->is_enabled()) {
return;
}
switch (object_id.id) {
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
Expand All @@ -749,8 +749,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,
Expand All @@ -777,9 +782,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
Expand Down
7 changes: 6 additions & 1 deletion libraries/RC_Channel/RC_Channel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -769,6 +769,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::DDS_EXTERNAL_CONTROL:
#endif
#if AP_AHRS_ENABLED
case AUX_FUNC::AHRS_TYPE:
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
Expand Down Expand Up @@ -1851,14 +1854,16 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch
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_DDS_ENABLED
#endif // AP_DDS_ENABLED

// do nothing for these functions
#if HAL_MOUNT_ENABLED
Expand Down

0 comments on commit f46bde5

Please sign in to comment.