Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/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()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement