From 76e5ae9b41b92548ddafe6716c877f893bcfe176 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Jun 2020 13:08:10 +1000 Subject: [PATCH] Copter: move mavlink set-position defines into mavlink library --- ArduCopter/GCS_Mavlink.cpp | 21 +++++++++++++++++++++ ArduCopter/defines.h | 8 -------- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 52f5a5e706..9176791797 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -945,6 +945,27 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) { + // for mavlink SET_POSITION_TARGET messages + constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE; + + constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE; + + constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE; + + constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = + POSITION_TARGET_TYPEMASK_YAW_IGNORE; + constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE = + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; + switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fb9ff1bcb1..cc097aab9d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -203,14 +203,6 @@ enum LoggingParameters { #define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe #define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize -// for mavlink SET_POSITION_TARGET messages -#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2)) -#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5)) -#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8)) -#define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<9) -#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10) -#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11) - // for PILOT_THR_BHV parameter #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0) #define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)