public void setPositionTargetGlobalInt(byte sysid, byte compid, bool pos, bool vel, bool acc, MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz)
{
// for mavlink SET_POSITION_TARGET messages
const ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
const ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
const ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8));
mavlink_set_position_target_global_int_t target = new mavlink_set_position_target_global_int_t()
{
target_system = sysid,
target_component = compid,
alt = (float) alt,
lat_int = (int) (lat*1e7),
lon_int = (int) (lng*1e7),
coordinate_frame = (byte) frame,
vx = (float) vx,
vy = (float) vy,
vz = (float) vz
};
target.type_mask = 7 + 56 + 448;
if (pos)
target.type_mask -= 7;
if (vel)
target.type_mask -= 56;
if (acc)
target.type_mask -= 448;
bool pos_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE)>0;
bool vel_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE)>0;
bool acc_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE)>0;
generatePacket((byte)MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, target, sysid, compid);
}