SHARE
TWEET

Untitled

a guest Apr 19th, 2019 103 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. # -*- coding: utf-8 -*-
  3.  
  4. from dronekit import connect, VehicleMode
  5. import time
  6. import math
  7.  
  8. MAV_FRAME_GLOBAL_INT = 5
  9.  
  10. connection_string = "tcp:192.168.3.6:14550"
  11.  
  12. #Define callback for `vehicle.attitude` observer
  13. last_attitude_cache = None
  14. def attitude_callback(self, attr_name, value):
  15.     # `attr_name` - the observed attribute (used if callback is used for multiple attributes)
  16.     # `self` - the associated vehicle object (used if a callback is different for multiple vehicles)
  17.     # `value` is the updated attribute value.
  18.     global last_attitude_cache
  19.     # Only publish when value changes
  20.     if value!=last_attitude_cache:
  21.         print " CALLBACK: Attitude changed to", value
  22.         last_attitude_cache=value
  23.  
  24. def global_yaw(vehicle, heading):
  25.     print "send mavlink command heading : %s" % heading
  26.     # Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified location.
  27.     msg = vehicle.message_factory.set_position_target_global_int_encode(
  28.         0,  # time_boot_ms (not used)
  29.         0, 0,  # target system, target component
  30.         MAV_FRAME_GLOBAL_INT,  # frame
  31.         0b101111111111,  # type_mask (only yaw enabled)
  32.         0,  # lat_int - X Position in WGS84 frame in 1e7 * meters
  33.         0,  # lon_int - Y Position in WGS84 frame in 1e7 * meters
  34.         0,  # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative,
  35.             # above terrain if GLOBAL_TERRAIN_ALT_INT
  36.         0,  # X velocity in NED frame in m/s
  37.         0,  # Y velocity in NED frame in m/s
  38.         0,  # Z velocity in NED frame in m/s
  39.         0, 0, 0,  # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
  40.         heading, # yaw
  41.         0)  # yaw_rate
  42.     # send command to vehicle
  43.     vehicle.send_mavlink(msg)
  44.     vehicle.flush()
  45.  
  46.  
  47. print("\nConnecting to vehicle on: %s" % connection_string)
  48. vehicle = connect(connection_string, wait_ready=True)
  49.  
  50. vehicle.wait_ready('autopilot_version')
  51.  
  52. print(" Global Location: %s" % vehicle.location.global_frame)
  53. print(" Global Location (relative altitude): %s" % vehicle.location.global_relative_frame)
  54. print(" Local Location: %s" % vehicle.location.local_frame)
  55.  
  56. print(" Heading: %s" % vehicle.heading)
  57. print(" Is Armable?: %s" % vehicle.is_armable)
  58. print(" System status: %s" % vehicle.system_status.state)
  59. print(" Mode: %s" % vehicle.mode.name)  # settable
  60. print(" Armed: %s" % vehicle.armed)  # settable
  61.  
  62. #print "\nAdd `attitude` attribute callback/observer on `vehicle`"
  63. #vehicle.add_attribute_listener('attitude', attitude_callback)
  64.  
  65. vehicle.mode = VehicleMode("GUIDED")
  66. vehicle.armed = True
  67.  
  68. while not vehicle.armed:
  69.     print " Waiting for arming..."
  70.     time.sleep(1)
  71. print " Vehicle is armed: %s" % vehicle.armed
  72.  
  73. for x in range(0, 20):
  74.     print "Attitude: %s" % vehicle.attitude
  75.     global_yaw(vehicle, math.radians(40))
  76.     time.sleep(1)
  77.  
  78.  
  79. vehicle.armed = False
  80.  
  81. while vehicle.armed:
  82.     print " Waiting for disarming..."
  83.     time.sleep(1)
  84. print " Vehicle is disarmed: %s" % vehicle.armed
  85.  
  86. #print " Remove Vehicle.attitude observer"
  87. # Remove observer added with `add_attribute_listener()` specifying the attribute and callback function
  88. #vehicle.remove_attribute_listener('attitude', attitude_callback)
  89.  
  90. vehicle.close()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top