Exploring Waypoint Control

Waypoints are transmitted to the APM through MAVLink commands. Transmission follows the protocol defined at Waypoint Protocol.

Using the mission_item_send function, a waypoint may be structured and transmitted to the APM through MAVLink. The mission_item_send function accepts multiple parameters and a specified action.

def mission_item_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z):

    Message encoding a mission item. This message is emitted to announce
    the presence of a mission item and to set a mission
    item on the system. The mission item can be either in
    x, y, z meters (type: LOCAL) or x:lat, y:lon,
    z:altitude. Local frame is Z-down, right handed (NED),
    global frame is Z-up, right handed (ENU).
    http://qgroundcontrol.org/mavlink/waypoint_protocol
  
    target_system             : System ID (uint8_t)
    target_component          : Component ID (uint8_t)
    seq                       : Sequence (uint16_t)
    frame                     : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t)
    command                   : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t)
    current                   : false:0, true:1 (uint8_t)
    autocontinue              : autocontinue to next wp (uint8_t)
    param1                    : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float)
    param2                    : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float)
    param3                    : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float)
    param4                    : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float)
    x                         : PARAM5 / local: x position, global: latitude (float)
    y                         : PARAM6 / y position: global: longitude (float)
    z                         : PARAM7 / z position: global: altitude (float)


    return self.send(self.mission_item_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z))

Mission item actions may be seen in the Common Message Documentation under MAV_CMD.

MAV_CMD_NAV_WAYPOINT

• Mission Param #1 Hold time in decimal seconds.
• Mission Param #2 Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)
• Mission Param #3 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.
• Mission Param #4 Desired yaw angle at MISSION
• Mission Param #5 Latitude
• Mission Param #6 Longitude
• Mission Param #7 Altitude

The MAV_CMD_NAV_WAYPOINT action description for Ardupilot is shown in the GCS_MAVLink library common.xml code.

NOTE: There is a conflict between the "mission_item_send" description and MAV_CMD_NAV_WAYPOINT description. Param 1 and 2 are flipped.

The "handle_message" function of GCS_MAVLink.pde processes the MAV_CMD_NAV_WAYPOINT parsing. Specifically, the case "MAVLINK_MSG_ID_MISSION_ITEM:" specifies how the mission_item is received from the APM code (GCS).

Through this code, it is shown that Params 2,3,4 from the MAV command are not used. Param 1 designates the Hold Time, following the same structure as defined in the Message Documentations, but not the mission_item_send function descriptions.

To define Yaw angle, a second waypoint should be generated at the same position to change orientation (TBD). To define Acceptance Radius, a parameter must be set on the APM, WPNAV_RADIUS (default is 2 meters).

MAV_CMD_CONDITION_YAW

• Mission Param #1 target angle: [0-360], 0 is north
• Mission Param #2 speed during yaw change:[deg per second]
• Mission Param #3 direction: negative: counter clockwise, positive: clockwise [-1,1]
• Mission Param #4 relative offset or absolute angle: [ 1,0]
• Mission Param #5 Empty
• Mission Param #6 Empty
• Mission Param #7 Empty

The MAV_CMD_CONDITION_YAW action description for Ardupilot is shown in the GCS_MAVLink library common.xml code.

The "do_yaw" function in the APM file commands_logic.pde is used to handle the condition yaw waypoint.