#!/usr/bin/env python
# -*- coding: utf-8 -*-

from dronekit import connect, VehicleMode
import time
import math

MAV_FRAME_GLOBAL_INT = 5

connection_string = "tcp:192.168.3.6:14550"

#Define callback for `vehicle.attitude` observer
last_attitude_cache = None
def attitude_callback(self, attr_name, value):
    # `attr_name` - the observed attribute (used if callback is used for multiple attributes)
    # `self` - the associated vehicle object (used if a callback is different for multiple vehicles)
    # `value` is the updated attribute value.
    global last_attitude_cache
    # Only publish when value changes
    if value!=last_attitude_cache:
        print " CALLBACK: Attitude changed to", value
        last_attitude_cache=value

def global_yaw(vehicle, heading):
    print "send mavlink command heading : %s" % heading
    # Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified location.
    msg = vehicle.message_factory.set_position_target_global_int_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        MAV_FRAME_GLOBAL_INT,  # frame
        0b101111111111,  # type_mask (only yaw 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,
            # above terrain if GLOBAL_TERRAIN_ALT_INT
        0,  # X velocity in NED frame in m/s
        0,  # Y velocity in NED frame in m/s
        0,  # Z velocity in NED frame in m/s
        0, 0, 0,  # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
        heading, # yaw
        0)  # yaw_rate
    # send command to vehicle
    vehicle.send_mavlink(msg)
    vehicle.flush()


print("\nConnecting to vehicle on: %s" % connection_string)
vehicle = connect(connection_string, wait_ready=True)

vehicle.wait_ready('autopilot_version')

print(" Global Location: %s" % vehicle.location.global_frame)
print(" Global Location (relative altitude): %s" % vehicle.location.global_relative_frame)
print(" Local Location: %s" % vehicle.location.local_frame)

print(" Heading: %s" % vehicle.heading)
print(" Is Armable?: %s" % vehicle.is_armable)
print(" System status: %s" % vehicle.system_status.state)
print(" Mode: %s" % vehicle.mode.name)  # settable
print(" Armed: %s" % vehicle.armed)  # settable

#print "\nAdd `attitude` attribute callback/observer on `vehicle`"
#vehicle.add_attribute_listener('attitude', attitude_callback)

vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True

while not vehicle.armed:
    print " Waiting for arming..."
    time.sleep(1)
print " Vehicle is armed: %s" % vehicle.armed

for x in range(0, 20):
    print "Attitude: %s" % vehicle.attitude
    global_yaw(vehicle, math.radians(40))
    time.sleep(1)


vehicle.armed = False

while vehicle.armed:
    print " Waiting for disarming..."
    time.sleep(1)
print " Vehicle is disarmed: %s" % vehicle.armed

#print " Remove Vehicle.attitude observer"
# Remove observer added with `add_attribute_listener()` specifying the attribute and callback function
#vehicle.remove_attribute_listener('attitude', attitude_callback)

vehicle.close()