Guiding and Controlling Copter

GUIDED mode is the recommended mode for flying Copter autonomously without a predefined a mission. It allows a Ground Control Station (GCS) or Companion Computer to control the vehicle “on the fly” and react to new events or situations as they occur.

This topic explains how you can control vehicle movement, and also how to send MAVLink commands to control vehicle orientation, region of interest, servos and other hardware. We also list a few functions that are useful for converting location and bearings from a global into a local frame of reference.

Most of the code can be observed running in Example: Guided Mode Movement and Commands (Copter).

Note

This topic is Copter specific. Plane apps typically use AUTO mode and dynamically modify missions as needed (Plane supports GUIDED mode but it is less full featured than on Copter).

Controlling vehicle movement

Copter movement can be controlled either by explicitly setting a target position, or by specifying the vehicle’s velocity components to guide it in a preferred direction.

Note

Changing to a new movement method is treated as a “mode change”. If you’ve set a yaw or region-of-interest value then this will be set to the default (vehicle faces the direction of travel).

Position control

Controlling the vehicle by explicitly setting the target position is useful when the final position is known/fixed.

The recommended method for position control is Vehicle.commands.goto(). This takes a Location argument for the target position in the global WGS84 coordinate system, but with altitude relative to the home location (home altitude = 0).

The method is used as shown below:

# Set mode to guided - this is optional as the goto method will change the mode if needed.
vehicle.mode = VehicleMode("GUIDED")

# Set the target location and then call flush()
a_location = Location(-34.364114, 149.166022, 30, is_relative=True)
vehicle.commands.goto(a_location)
vehicle.flush()

Vehicle.commands.goto() can be interrupted by a later command, and does not provide any functionality to indicate when the vehicle has reached its destination. Developers can use either a time delay or measure proximity to the target to give the vehicle an opportunity to reach its destination. The Example: Guided Mode Movement and Commands (Copter) shows both approaches.

When moving the vehicle you can send a separate command to control the speed (and other vehicle behaviour).

Tip

You can also set the position by sending the MAVLink commands SET_POSITION_TARGET_GLOBAL_INT or SET_POSITION_TARGET_LOCAL_NED, specifying a type_mask bitmask that enables the position parameters. The main difference between these commands is that the former allows you to specify the location relative to the “global” frames (like Vehicle.commands.goto()), while the later lets you specify the location in NED co-ordinates relative to the home location. For more information on these options see the example code: goto_position_target_global_int() and goto_position_target_local_ned().

Velocity control

Controlling vehicle movement using velocity is much smoother than using position when there are likely to be many updates (for example when tracking moving objects).

The function send_ned_velocity() below generates a SET_POSITION_TARGET_LOCAL_NED MAVLink message which is used to directly specify the speed components of the vehicle.

def send_ned_velocity(velocity_x, velocity_y, velocity_z):
    """
    Move vehicle in direction based on specified velocity vectors.
    """
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED, # frame
        0b0000111111000111, # type_mask (only speeds enabled)
        0, 0, 0, # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
        0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()

The type_mask parameter is a bitmask that indicates which of the other parameters in the message are used/ignored by the vehicle (0 means that the dimension is enabled, 1 means ignored). In the example the value 0b0000111111000111 is used to enable the velocity components.

The speed components velocity_x and velocity_y are parallel to the North and East directions (not to the front and side of the vehicle). The velocity_z component is perpendicular to the plane of velocity_x and velocity_y, with a positive value towards the ground, following the right-hand convention. For more information about the mavutil.mavlink.MAV_FRAME_BODY_NED frame of reference, see this wikipedia article on NED.

The code fragment below shows how to call this method:

# Set up velocity mappings
# velocity_x > 0 => fly North
# velocity_x < 0 => fly South
# velocity_y > 0 => fly East
# velocity_y < 0 => fly West
# velocity_z < 0 => ascend
# velocity_z > 0 => descend
SOUTH=-2
UP=-0.5   #NOTE: up is negative!

#Fly south and up.
send_ned_velocity(SOUTH,0,UP)

The command can be interrupted by a later movement command. When moving the vehicle you can send separate commands to control the yaw (and other behaviour).

Tip

You can also control the velocity using the SET_POSITION_TARGET_GLOBAL_INT MAVLink command in almost exactly the same way (there is no real benefit in sending one command over the other). For more information on this option see send_global_velocity() in the example code.

Acceleration and force control

ArduPilot does not currently support controlling the vehicle by specifying acceleration/force components.

Note

The SET_POSITION_TARGET_GLOBAL_INT and SET_POSITION_TARGET_LOCAL_NED MAVLink commands allow you to specify the acceleration, force and yaw. However, commands setting these parameters are ignored by the vehicle.

Guided mode commands

This section explains how to send MAVLink commands, what commands can be sent, and lists a number of real examples you can use in your own code.

Sending messages/commands

MAVLink commands are sent by first using message_factory to encode the message and then calling send_mavlink and flush() to send them.

message_factory() uses a factory method for the encoding. The name of this method will always be the lower case version of the message/command name with _encode appended. For example, to encode a SET_POSITION_TARGET_LOCAL_NED message we call message_factory.set_position_target_local_ned_encode() with values for all the message fields as arguments:

msg = vehicle.message_factory.set_position_target_local_ned_encode(
    0,       # time_boot_ms (not used)
    0, 0,    # target system, target component
    mavutil.mavlink.MAV_FRAME_BODY_NED, # frame
    0b0000111111000111, # type_mask (only speeds enabled)
    0, 0, 0, # x, y, z positions
    velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
    0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
    0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()

There is no need to specify the system id, component id or sequence number of messages (if defined in the message type) as the API will set these appropriately when the message is sent.

In Copter, the COMMAND_LONG message can be used send/package a number of different supported MAV_CMD commands. The factory function is again the lower case message name with suffix _encode (message_factory.command_long_encode). The message parameters include the actual command to be sent (in the code fragment below MAV_CMD_CONDITION_YAW) and its fields.

msg = vehicle.message_factory.command_long_encode(
    0, 0,    # target system, target component
    mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
    0, #confirmation
    heading,    # param 1, yaw in degrees
    0,          # param 2, yaw speed deg/s
    1,          # param 3, direction -1 ccw, 1 cw
    is_relative, # param 4, relative offset 1, absolute angle 0
    0, 0, 0)    # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()

Supported commands

Copter Commands in Guided Mode lists all the commands that can be sent to Copter in GUIDED mode (in fact most of the commands can be sent in any mode!)

DroneKit-Python provides a friendly Python API that abstracts many of the commands. Where possible you should use the API rather than send messages directly. For example it is better to use Vehicle.commands.takeoff() than to explicitly send the MAV_CMD_NAV_TAKEOFF command.

Some of the MAV_CMD commands that you might want to send include: MAV_CMD_CONDITION_YAW, MAV_CMD_DO_CHANGE_SPEED, MAV_CMD_DO_SET_HOME, MAV_CMD_DO_SET_ROI, MAV_CMD_DO_SET_SERVO, MAV_CMD_DO_REPEAT_SERVO, MAV_CMD_DO_SET_RELAY, MAV_CMD_DO_REPEAT_RELAY, MAV_CMD_DO_FENCE_ENABLE, MAV_CMD_DO_PARACHUTE, MAV_CMD_DO_GRIPPER, MAV_CMD_MISSION_START. These would be sent in a COMMAND_LONG message as discussed above.

Setting the Yaw

The vehicle “yaw” is the direction that the vehicle is facing in the horizontal plane. By default (after you set the mode or change the command used for controlling movement) the yaw of the vehicle will face the direction of travel.

In Copter the yaw need not match the direction of travel. You can set the yaw direction using the MAV_CMD_CONDITION_YAW command, encoded in a COMMAND_LONG message as shown below.

def condition_yaw(heading, relative=False):
    if relative:
        is_relative=1 #yaw relative to direction of travel
    else:
        is_relative=0 #yaw is an absolute angle
    # create the CONDITION_YAW command using command_long_encode()
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
        0, #confirmation
        heading,    # param 1, yaw in degrees
        0,          # param 2, yaw speed deg/s
        1,          # param 3, direction -1 ccw, 1 cw
        is_relative, # param 4, relative offset 1, absolute angle 0
        0, 0, 0)    # param 5 ~ 7 not used
    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()

The command allows you to specify that whether the heading is an absolute angle in degrees (0 degrees is North) or a value that is relative to the previously set heading.

Note

  • At time of writing there is no safe way to return to the default yaw “face direction of travel” behaviour.
  • Setting the ROI may work to get yaw to track a particular point (depending on the gimbal setup).

Setting the speed

Send MAV_CMD_DO_CHANGE_SPEED to change the current speed (metres/second) when travelling to a point.

def set_speed(speed):
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, #command
        0, #confirmation
        0, #param 1
        speed, # speed in metres/second
        0, 0, 0, 0, 0 #param 3 - 7
        )

    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()

The command is useful when setting the vehicle position directly. It is not needed when controlling movement using velocity vectors.

Note

In AC3.2.1 Copter will accelerate to the target speed across the journey and then decelerate as it reaches the target. In AC3.3 the speed changes immediately.

Setting the ROI

Send the MAV_CMD_DO_SET_ROI command to point camera gimbal at a specified region of interest (Location). The vehicle may also turn to face the ROI.

def set_roi(location):
    # create the MAV_CMD_DO_SET_ROI command
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
        0, #confirmation
        0, 0, 0, 0, #params 1-4
        location.lat,
        location.lon,
        location.alt
        )
    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()

New in version Copter: 3.2.1. You can explicitly reset the ROI by sending the MAV_CMD_DO_SET_ROI command with zero in all parameters. The front of the vehicle will then follow the direction of travel.

The ROI (and yaw) is also reset when the mode, or the command used to control movement, is changed.

Setting the home location

Send the MAV_CMD_DO_SET_HOME command to set the home location to either the current location or a specified location.

def set_home(aLocation, aCurrent=1):
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_CMD_DO_SET_HOME, #command
        0, #confirmation
        aCurrent, #param 1: 1 to use current position, 2 to use the entered values.
            0, 0, 0, #params 2-4
        aLocation.lat,
        aLocation.lon,
        aLocation.alt
        )
    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()

The home location is updated immediately in ArduPilot, but the change may not appear in the GCS/Mission Planner. You can force an update by reading the mission commands (this works, because the home location is currently implemented as the 0th waypoint command):

# Set new Home location to current location
set_home(vehicle.location)
# Reloads the home location in GCSs
cmds = vehicle.commands
cmds.download()
cmds.wait_valid()
print " Home WP: %s" % cmds[0]

Command acknowledgements and response values

ArduPilot typically sends a command acknowledgement indicating whether a command was received, and whether it was accepted or rejected. At time of writing there is no way to intercept this acknowledgement in the API (#168).

Some MAVLink messages request information from the autopilot, and expect the result to be returned in another message. At time of writing you can send the request (provided the message is handled by the AutoPilot in GUIDED mode) but there is no way to intercept the response in DroneKit-Python (#169).

Frame conversion functions

The functions in this section help convert between different frames-of-reference. In particular they make it easier to navigate in terms of “metres from the current position” when using commands that take absolute positions in decimal degrees.

The methods are approximations only, and may be less accurate over longer distances, and when close to the Earth’s poles.

def get_location_metres(original_location, dNorth, dEast):
    """
    Returns a Location object containing the latitude/longitude `dNorth` and `dEast` metres from the
    specified `original_location`. The returned Location has the same `alt and `is_relative` values
    as `original_location`.

    The function is useful when you want to move the vehicle around specifying locations relative to
    the current vehicle position.

    The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.

    For more information see:
    http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters
    """
    earth_radius=6378137.0 #Radius of "spherical" earth
    #Coordinate offsets in radians
    dLat = dNorth/earth_radius
    dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))

    #New position in decimal degrees
    newlat = original_location.lat + (dLat * 180/math.pi)
    newlon = original_location.lon + (dLon * 180/math.pi)
    return Location(newlat, newlon,original_location.alt,original_location.is_relative)
def get_distance_metres(aLocation1, aLocation2):
    """
    Returns the ground distance in metres between two Location objects.

    This method is an approximation, and will not be accurate over large distances and close to the
    earth's poles. It comes from the ArduPilot test code:
    https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py
    """
    dlat            = aLocation2.lat - aLocation1.lat
    dlong           = aLocation2.lon - aLocation1.lon
    return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
def get_bearing(aLocation1, aLocation2):
    """
    Returns the bearing between the two Location objects passed as parameters.

    This method is an approximation, and may not be accurate over large distances and close to the
    earth's poles. It comes from the ArduPilot test code:
    https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py
    """
    off_x = aLocation2.lon - aLocation1.lon
    off_y = aLocation2.lat - aLocation1.lat
    bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
    if bearing < 0:
        bearing += 360.00
    return bearing;

Tip

The common.py file in the ArduPilot test code may have other functions that you will find useful.