From e0d181a793011b1fb9365c9c3fa5f809802db3f7 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 16 Mar 2021 18:28:28 +0100 Subject: [PATCH] Copter: use MAVLink POSITION_TARGET_TYPEMASK --- ArduCopter/GCS_Mavlink.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 8b62022db4..7fff1278d3 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -101,12 +101,15 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() if (!target.change_alt_frame(Location::AltFrame::ABSOLUTE)) { return; } - + static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; + static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE; mavlink_msg_position_target_global_int_send( chan, AP_HAL::millis(), // time_boot_ms MAV_FRAME_GLOBAL, // targets are always global altitude - 0xFFF8, // ignore everything except the x/y/z components + TYPE_MASK, // ignore everything except the x/y/z components target.lat, // latitude as 1e7 target.lng, // longitude as 1e7 target.alt * 0.01f, // altitude is sent as a float @@ -137,16 +140,21 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() // we don't have a local target when in angle mode return; case ModeGuided::SubMode::WP: - type_mask = 0x0FF8; // ignore everything except position + type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres break; case ModeGuided::SubMode::Velocity: - type_mask = 0x0FC7; // ignore everything except velocity + type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s break; case ModeGuided::SubMode::TakeOff: case ModeGuided::SubMode::PosVel: - type_mask = 0x0FC0; // ignore everything except position & velocity + type_mask = POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position & velocity target_pos = copter.wp_nav->get_wp_destination() * 0.01f; target_vel = copter.flightmode->get_desired_velocity() * 0.01f; break;