public static void GuidedPosition(double lat, double lng, double alt) { //const int MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2)); const int MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5)); const int MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8)); MAVLink.mavlink_set_position_target_global_int_t cmd = new MAVLink.mavlink_set_position_target_global_int_t(); // cmd.target_system = sysID; // cmd.target_component = compID; cmd.target_system = 0; cmd.target_component = 1; cmd.coordinate_frame = 5; // MAV_FRAME_GLOBAL_INT // Use only speed cmd.type_mask = MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; cmd.lat_int = (int)(lat * 10000000); cmd.lon_int = (int)(lng * 10000000); cmd.alt = (float)alt; byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, cmd); SendPacket(commandBytes); }
// Set velocity, in m/s private static void SetVelocity(double vx, double vy, double vz) { const int MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2)); // const int MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1<<3) | (1<<4) | (1<<5)); const int MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8)); MAVLink.mavlink_set_position_target_global_int_t cmd = new MAVLink.mavlink_set_position_target_global_int_t(); // cmd.target_system = sysID; // cmd.target_component = compID; cmd.target_system = 1; cmd.target_component = 0; cmd.coordinate_frame = 5; // MAV_FRAME_GLOBAL_INT // Use only speed cmd.type_mask = MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; cmd.vx = (float)vx; cmd.vy = (float)vy; cmd.vz = (float)vz; byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, cmd); SendPacket(commandBytes); }