Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_DDS: External Control enable #28429

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+70 −5
Open

AP_DDS: External Control enable #28429

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 23 additions & 5 deletions libraries/AP_DDS/AP_DDS_Client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Common/AP_FWVersion.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"
Expand Down Expand Up @@ -709,6 +710,11 @@ 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();
if (!external_control->is_enabled()) {
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: {
Expand Down Expand Up @@ -804,6 +810,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: {
Expand All @@ -814,8 +822,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()) {
tizianofiorenzani marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Collaborator

@Ryanf55 Ryanf55 Nov 5, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we instead put an arm function into the external control library, and check is_enabled there, then call AP::arming().arm there?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same for the other services though, right?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yep, I think so. For the over services, that could/should be it's own PR. I'll do that now.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

so you would call external_control->arm(), which would fail if either the external control is disabled or arming failed.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here's the implementation. Once this merges, rebase.
#28609

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 Down Expand Up @@ -844,9 +857,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
4 changes: 4 additions & 0 deletions libraries/AP_DDS/AP_DDS_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,10 @@
#define AP_DDS_ENABLED 1
#endif

#ifndef AP_EXTERNAL_CONTROL_ENABLED
tizianofiorenzani marked this conversation as resolved.
Show resolved Hide resolved
#define AP_EXTERNAL_CONTROL_ENABLED 1
#endif

// UDP only on SITL for now
#ifndef AP_DDS_UDP_ENABLED
#define AP_DDS_UDP_ENABLED AP_DDS_ENABLED && AP_NETWORKING_ENABLED
Expand Down
21 changes: 21 additions & 0 deletions libraries/AP_ExternalControl/AP_ExternalControl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -40,6 +60,7 @@ class AP_ExternalControl

private:
static AP_ExternalControl *singleton;
bool enabled {true};
};


Expand Down
21 changes: 21 additions & 0 deletions libraries/RC_Channel/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
tizianofiorenzani marked this conversation as resolved.
Show resolved Hide resolved
#include <AP_DDS/AP_DDS_config.h>
#define SWITCH_DEBOUNCE_TIME_MS 200

const AP_Param::GroupInfo RC_Channel::var_info[] = {
Expand Down Expand Up @@ -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
tizianofiorenzani marked this conversation as resolved.
Show resolved Hide resolved
case AUX_FUNC::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 @@ -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:
Expand Down
1 change: 1 addition & 0 deletions libraries/RC_Channel/RC_Channel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading