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.

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

Copter: GCSMavlink Guided inputs check force_set and reject #18720

Merged
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
14 changes: 14 additions & 0 deletions ArduCopter/GCS_Mavlink.cpp
Expand Up @@ -1009,6 +1009,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
POSITION_TARGET_TYPEMASK_YAW_IGNORE;
constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE =
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_FORCE_SET =
POSITION_TARGET_TYPEMASK_FORCE_SET;

switch (msg.msgid) {

Expand Down Expand Up @@ -1119,6 +1121,12 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
bool force_set = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;
Copy link
Contributor

Choose a reason for hiding this comment

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

Should be const, but not a blocker :-)

Copy link
Member Author

Choose a reason for hiding this comment

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

ahh but then you will make me change all the ones above it :)


// force_set true, do not accept command
if (force_set) {
break;
}

// prepare position
Vector3f pos_vector;
Expand Down Expand Up @@ -1207,6 +1215,12 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
bool force_set = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;

// force_set true, do not accept command
if (force_set) {
break;
}

// extract location from message
Location loc;
Expand Down