How to use the pymavlink.mavutil.mavlink.MAV_CMD_NAV_WAYPOINT function in pymavlink

loc.alt, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        m = self.mav.recv_match(type='MISSION_ACK', blocking=True, timeout=5)
        if m is None:
            raise NotAchievedException("Did not get MISSION_ACK")
        if m.type != mavutil.mavlink.MAV_MISSION_ERROR:
            raise NotAchievedException("Did not get appropriate error")

        self.start_subtest("Enter guided and flying somewhere constant")
        self.change_mode("GUIDED")
        self.mav.mav.mission_item_int_send(
            target_system,
            target_component,
            0, # seq
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
            mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
            2, # current - guided-mode request
            0, # autocontinue
            0, # p1
            0, # p2
            0, # p3
            0, # p4
            int(loc.lat *1e7), # latitude
            int(loc.lng *1e7), # longitude
            desired_relative_alt, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        m = self.mav.recv_match(type='MISSION_ACK', blocking=True, timeout=5)
        if m is None:
            raise NotAchievedException("Did not get MISSION_ACK")
        if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED:
            raise NotAchievedException("Did not get accepted response")
        self.wait_location(loc, accuracy=100) # based on loiter radius

self.progress("Expecting request for rally item 2")
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 2)

            self.progress("Answering request for rally point 2")
            items[2].pack(self.mav.mav)
            self.mav.mav.send(items[2])
            self.progress("Expecting mission ack for rally")
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.progress("Answering request for waypoints item 1")
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 *1e7), # latitude
                int(1.2000 *1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 2)

            self.progress("Answering request for waypoints item 2")
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,

print " Define/add new commands."
    # Add new commands. The meaning/order of the parameters is documented in the Command class. 
     
    #Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))

    #Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
    point1 = get_location_metres(aLocation, aSize, -aSize)
    point2 = get_location_metres(aLocation, aSize, aSize)
    point3 = get_location_metres(aLocation, -aSize, aSize)
    point4 = get_location_metres(aLocation, -aSize, -aSize)
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 11))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 12))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 13))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14))
    #add dummy waypoint "5" at point 4 (lets us know when have reached destination)
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14))    

    print " Upload new commands to vehicle"
    cmds.upload()

longitude = float(args[1])
            altitude = float(args[2])
            latlon = (latitude, longitude)
        else:
            latlon = self.mpstate.click_location
            if latlon is None:
                print("No map click position available")
                return
            altitude = float(args[0])

        print("Guided %s %s" % (str(latlon), str(altitude)))
        self.master.mav.mission_item_int_send (self.settings.target_system,
                                           self.settings.target_component,
                                           0,
                                           self.module('wp').get_default_frame(),
                                           mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                                           2, 0, 0, 0, 0, 0,
                                           int(latlon[0]*1.0e7),
                                           int(latlon[1]*1.0e7),
                                           altitude)

# Set the LocationGlobal to head towards
            a_location = LocationGlobal(-34.364114, 149.166022, 30)
            vehicle.commands.goto(a_location)

        :param LocationGlobal location: The target location.
        '''
        if isinstance(location, LocationGlobalRelative):
            frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
        elif isinstance(location, LocationGlobal):
            frame = mavutil.mavlink.MAV_FRAME_GLOBAL
        else:
            raise APIException('Expecting location to be LocationGlobal or LocationGlobalRelative.')

        self._vehicle._master.mav.mission_item_send(0, 0, 0,
                                                    frame,
                                                    mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                                                    2, 0, 0, 0, 0, 0,
                                                    location.lat, location.lon, location.alt)

rospy.logwarn("Waypoint write not in progress, but received a request for a waypoint")
                else:
                    waypoint = self.waypoint_buffer[msg.seq]
                    frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
                    if msg.seq == 0:
                        # Waypoint zero seems to be special, and uses the
                        # GLOBAL frame. It also is magically reset in the
                        # firmware, so this probably doesn't matter.
                        frame = mavutil.mavlink.MAV_FRAME_GLOBAL
                    self.last_waypoint_message_time = time()
                    self.conn.mav.mission_item_send(
                        self.conn.target_system,
                        self.conn.target_component,
                        msg.seq,
                        frame,
                        mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                        1 if msg.seq == 1 else 0,  # Set current
                        1,  # Auto continue after this waypoint
                        1.0,  # "reached waypoint" is +/- 1.0m
                        5.0,  # Stay for 5 secs then move on
                        1.0,  # Stay within 1.0m for LOITER
                        0,  # Face north on arrival
                        waypoint.latitude,  # Latitude
                        waypoint.longitude,  # Longitude
                        waypoint.altitude)  # Altitude
            elif msg_type == "MISSION_ACK":
                if not self.waypoint_write_in_progress:
                    rospy.logwarn("Did not expect MISSION_ACK no write in progress")
                # NOTE: APM is suppose to return MAV_CMD_ACK_OK, but it seems
                # to return 0
                elif msg.type not in (0, mavutil.mavlink.MAV_CMD_ACK_OK):
                    rospy.logerr("Bad MISSION_ACK: %d", msg.type)

def get_home(self):
        '''get home location'''
        if 'HOME_POSITION' in self.master.messages:
            h = self.master.messages['HOME_POSITION']
            return mavutil.mavlink.MAVLink_mission_item_message(self.target_system,
                                                                self.target_component,
                                                                0,
                                                                0,
                                                                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                                                                0, 0, 0, 0, 0, 0,
                                                                h.latitude*1.0e-7, h.longitude*1.0e-7, h.altitude*1.0e-3)
        if self.wploader.count() > 0:
            return self.wploader.wp(0)
        return None

wp_Last_Altitude):  #--- [m]    Target Altitude
    """
    Upload the mission with the last WP as given and outputs the ID to be set
    """
    # Get the set of commands from the vehicle
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()

    # Save the vehicle commands to a list
    missionlist=[]
    for cmd in cmds:
        missionlist.append(cmd)

    # Modify the mission as needed. For example, here we change the
    wpLastObject = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 
                           wp_Last_Latitude, wp_Last_Longitude, wp_Last_Altitude)
    missionlist.append(wpLastObject)

    # Clear the current mission (command is sent when we call upload())
    cmds.clear()

    #Write the modified mission and flush to the vehicle
    for cmd in missionlist:
        cmds.add(cmd)
    cmds.upload()
    
    return (cmds.count)

def create_mission(drone, waypoints):
	cmds = drone.commands
	cmds.wait_ready()
	cmds.clear()

	cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoints[0]['lat'], waypoints[0]['lon'], 10))

	for (i, wp) in enumerate(waypoints):
		cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, wp['lat'], wp['lon'], 10))

	cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoints[-1]['lat'], waypoints[-1]['lon'], 10))
	print('uploading mission...')

	cmds.upload()

	print('mission uploaded')

	return

'''estimate time remaining in mission in seconds'''
        idx = wpnum
        if wpnum >= self.module('wp').wploader.count():
            return 0
        distance = 0
        done = set()
        while idx < self.module('wp').wploader.count():
            if idx in done:
                break
            done.add(idx)
            w = self.module('wp').wploader.wp(idx)
            if w.command == mavutil.mavlink.MAV_CMD_DO_JUMP:
                idx = int(w.param1)
                continue
            idx += 1
            if (w.x != 0 or w.y != 0) and w.command in [mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                                                        mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
                                                        mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
                                                        mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
                                                        mavutil.mavlink.MAV_CMD_NAV_LAND,
                                                        mavutil.mavlink.MAV_CMD_NAV_TAKEOFF]:
                distance += mp_util.gps_distance(lat, lon, w.x, w.y)
                lat = w.x
                lon = w.y
                if w.command == mavutil.mavlink.MAV_CMD_NAV_LAND:
                    break
        return distance / speed