Hi,
I wrote a simple program to test the drone which does the following:
- takes off and get home position lat long alt
- moveBy couple meters
- moveTo home position saved from step 1 and land
I could not get the drone to follow the moveTo command properly, most of the time it stay in the same spot after moveBy instead of moving back to home position. The code works fine in sphinx so I’m not too sure what the issue is. I did hard code the altitude for the moveBy if that may have causes a problem. Appreciate the help.
# Example program to take off, moveby a certain distance and land while taking burst images.
from olympe.messages.camera import (
set_camera_mode,
set_photo_mode,
take_photo,
photo_progress,
)
import olympe
import olympe_deps as od
from olympe.messages.skyctrl.CoPiloting import setPilotingSource
from olympe.enums.ardrone3.Piloting import MoveTo_Orientation_mode
from olympe.messages.ardrone3.PilotingState import moveToChanged, FlyingStateChanged, PositionChanged, AttitudeChanged
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
from olympe.messages.ardrone3.PilotingState import GpsLocationChanged
from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing, moveTo, Circle, PCMD
import os
import re
import requests
import shutil
import tempfile
import xml.etree.ElementTree as ET
import time
# Drone IP
#ANAFI_IP = "192.168.42.1" # direct drone ip
ANAFI_IP = "192.168.53.1:180" # skycontroller ip
SPHINX_IP = "10.202.0.1"
photo_on = False
# Drone web server URL
ANAFI_URL = "http://{}/".format(ANAFI_IP)
# Drone media web API URL
ANAFI_MEDIA_API_URL = ANAFI_URL + "api/v1/media/medias/"
XMP_TAGS_OF_INTEREST = (
"CameraRollDegree",
"CameraPitchDegree",
"CameraYawDegree",
"CaptureTsUs",
# NOTE: GPS metadata is only present if the drone has a GPS fix
# (i.e. they won't be present indoor)
"GPSLatitude",
"GPSLongitude",
"GPSAltitude",
)
def take_photo_burst(drone):
# take a photo burst and get the associated media_id
photo_saved = drone(photo_progress(result="photo_saved", _policy="wait"))
drone(take_photo(cam_id=0)).wait()
photo_saved.wait()
media_id = photo_saved.received_events().last().args["media_id"]
print("done taking photo burst!!")
# download the photos associated with this media id
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
#download_dir = tempfile.mkdtemp()
download_dir = 'images/'
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
print("Getting image name: ", ANAFI_URL + resource["url"])
download_path = os.path.join(download_dir, resource["resource_id"])
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
# parse the xmp metadata
with open(download_path, "rb") as image_file:
print("Opening ", download_path)
image_data = image_file.read()
image_xmp_start = image_data.find(b"<x:xmpmeta")
image_xmp_end = image_data.find(b"</x:xmpmeta")
image_xmp = ET.fromstring(image_data[image_xmp_start : image_xmp_end + 12])
#print("IMAGE_XMP: ", image_xmp[0][0])
for image_meta in image_xmp[0][0]:
xmp_tag = re.sub(r"{[^}]*}", "", image_meta.tag)
xmp_value = image_meta.text
# only print the XMP tags we are interested in
if xmp_tag in XMP_TAGS_OF_INTEREST:
print("Resource ID: ", resource["resource_id"], xmp_tag, xmp_value)
def setup_photo_burst_mode(drone):
drone(set_camera_mode(cam_id=0, value="photo")).wait()
# For the file_format: jpeg is the only available option
# dng is not supported in burst mode
drone(
set_photo_mode(
cam_id=0,
mode="burst",
format="rectilinear",
file_format="jpeg",
burst="burst_14_over_1s",
bracketing="preset_1ev",
capture_interval=0.0,
)
).wait()
print("Setup function done!!!!!")
########################################################## main ###########################################################
#### Connect to the drone ####
# Skycontroller connection, make sure correct ANAFI_IP also set.
#drone = olympe.Drone("192.168.53.1", mpp=True, drone_type=od.ARSDK_DEVICE_TYPE_ANAFI4K, loglevel=0)
#drone.connection()
#drone(setPilotingSource(source="Controller")).wait()
# SPHINX connection
drone = olympe.Drone(SPHINX_IP, loglevel=0)
drone.connection()
# Call function to set photo settings
setup_photo_burst_mode(drone)
#### Take off ####
drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (
GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
>> (
TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")
)
)
).wait()
# Clear the screen.
os.system('clear')
print("%%%%%%%%% Drone finished taking off %%%%%%%%%%%%%%%%%%%%%")
# Get drone position
TakeOffLocation = drone.get_state(GpsLocationChanged)
# take 14 photo burst over 1 sec
if photo_on:
print("Taking photo burst...")
take_photo_burst(drone)
d = input("hit enter to continue: ")
#### Moveby Waypoint 1 ####
dX = 7
dY = 0
dZ = 0 # to go up is negative
drone(
moveBy(dX,dY,dZ,dPsi=0)
>> PCMD(1, 0, 0, 0, 0, 0)
# Wait for drone to be in hovering state
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
print("%%%%%% Finished moving to WP 1 (wait 3 sec before getting position) %%%%%")
# Wait a little, for some reason the drone may still be moving ??
time.sleep(3)
# Get drone position
WayPoint1_Location = drone.get_state(GpsLocationChanged)
# take 14 photo burst over 1 sec
if photo_on:
print("Taking photo burst...")
take_photo_burst(drone)
d = input("hit enter to continue: ")
#### Go back home ####
Lat = TakeOffLocation["latitude"]
Longi = TakeOffLocation["longitude"]
Alt = 10
drone(
moveTo(Lat,Longi,Alt, MoveTo_Orientation_mode.NONE, 0.0)
>> moveToChanged(Lat,Longi,Alt,orientation_mode=MoveTo_Orientation_mode.NONE, status='DONE', _policy='check_wait')
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
print("%%% Finished moving home (wait 3 sec before getting position) %%%%")
# Wait a little, for some reason the drone is still moving ??
time.sleep(4)
# Get drone position
drone_location = drone.get_state(GpsLocationChanged)
# take 14 photo burst over 1 sec
if photo_on:
print("Taking photo burst...")
take_photo_burst(drone)
d = input("hit enter to continue: ")
#### Land the drone ####
drone(
Landing()
>> FlyingStateChanged(state="landed", _timeout=5)
).wait()
print("%%%%%%%%%%%%%%%%%%%% Finished landing %%%%%%%%%%%%%%%%%")
# The program prints finished landing before it actually lands ?
drone.disconnection()