gist.py

import time
import math

import pymavlink
import droneapi.lib
from pymavlink import mavutil
from droneapi.lib import VehicleMode

DEBUG = True
GPS_3D_FIX = 3

vehicle = None

def attitude_callback(event):
    print "Yaw: {}".format(vehicle.attitude.yaw)

def location_callback(event):
    print "Altitude: {}".format(vehicle.location.alt)

def initialize(api):
    global vehicle

    vehicle = api.get_vehicles()[0]

    vehicle.add_attribute_observer('attitude', attitude_callback)
    vehicle.add_attribute_observer('location', location_callback)

    # Wait for GPS fix
    wait_for_condition(lambda: vehicle.gps_0.fix_type != GPS_3D_FIX)

    if DEBUG:
        vehicle.parameters['ARMING_CHECK'] = 0
        vehicle.flush()

def startup(altitude):
    change_mode('GUIDED')
    wait_for_condition(lambda: vehicle.mode.name == "GUIDED")

    arm()
    wait_for_condition(lambda: vehicle.armed)

    takeoff(altitude)
    wait_for_condition(lambda: vehicle.location.alt >= altitude * .95)

    # This makes yawing work
    # send_global_velocity(0, 0, 0)

def change_mode(mode):
    vehicle.mode = VehicleMode(mode)
    vehicle.flush()

def arm():
    if vehicle.armed:
        return True

    vehicle.armed = True
    vehicle.flush()
    return True

def takeoff(altitude):
    """
    To get the drone off the ground, use: `startup(altitude)`.

    Adds the takeoff command to the drone's command list.
    """
    vehicle.commands.takeoff(altitude)
    vehicle.flush()

def send_global_velocity(velocity_x, velocity_y, velocity_z):
    """
    Move vehicle in direction based on specified velocity vectors.

    This uses the SET_POSITION_TARGET_GLOBAL_INT command with type mask enabling only
    velocity components (https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_GLOBAL_INT).

    See the above link for information on the type_mask (0=enable, 1=ignore).
    At time of writing, acceleration and yaw bits are ignored.
    """
    msg = vehicle.message_factory.set_position_target_global_int_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
        0b0000111111000111, # type_mask (only speeds enabled)
        0, # lat_int - X Position in WGS84 frame in 1e7 * meters
        0, # lon_int - Y Position in WGS84 frame in 1e7 * meters
        0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative)
           #     - Altitude above terrain if GLOBAL_TERRAIN_ALT_INT
        velocity_x, # X velocity in NED frame in m/s
        velocity_y, # Y velocity in NED frame in m/s
        velocity_z, # Z velocity in NED frame in m/s
        0, 0, 0, # afx, afy, afz 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()

def condition_yaw(heading, relative=False):
    """
    Send MAV_CMD_CONDITION_YAW message to point vehicle at a specified heading
    (in degrees).

    This method sets an absolute heading by default, but you can set the
    `relative` parameter to `True` to set yaw relative to the current yaw
    heading.

    By default the yaw of the vehicle will follow the direction of travel.
    After setting the yaw using this function there is no way to return to the
    default yaw "follow direction of travel" behaviour
    (https://github.com/diydrones/ardupilot/issues/2427)

    For more information see:
        http://copter.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_condition_yaw
    """
    # This makes yawing work
    # send_global_velocity(0, 0, 0)

    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
        abs(heading),    # param 1, yaw in degrees
        0,          # param 2, yaw speed deg/s
        math.copysign(1, heading),          # 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()

def wait_for_condition(callback, max_sleep_time=10.0, sleep_interval=0.3):
    start_time = time.time()
    while time.time() - start_time < max_sleep_time:
        if callback():
            return True
        time.sleep(sleep_interval)
    return False

def variance(x1, x2):
    return abs(x1 - x2) / ((x1 + x2) / 2.0)

initialize(local_connect())
startup(50)
condition_yaw(100, True)
time.sleep(100)