<s> """<STR_LIT>""" <EOL> from dronekit import connect , VehicleMode , LocationGlobal , LocationGlobalRelative <EOL> from pymavlink import mavutil <EOL> import time <EOL> import math <EOL> import argparse <EOL> parser = argparse . ArgumentParser ( description = '<STR_LIT>' ) <EOL> parser . add_argument ( '<STR_LIT>' , <EOL> help = "<STR_LIT>" ) <EOL> args = parser . parse_args ( ) <EOL> connection_string = args . connect <EOL> sitl = None <EOL> if not args . connect : <EOL> print "<STR_LIT>" <EOL> from dronekit_sitl import SITL <EOL> sitl = SITL ( ) <EOL> sitl . download ( '<STR_LIT>' , '<STR_LIT>' , verbose = True ) <EOL> sitl_args = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] <EOL> sitl . launch ( sitl_args , await_ready = True , restart = True ) <EOL> connection_string = '<STR_LIT>' <EOL> print '<STR_LIT>' % connection_string <EOL> vehicle = connect ( connection_string , wait_ready = True ) <EOL> def arm_and_takeoff ( aTargetAltitude ) : <EOL> """<STR_LIT>""" <EOL> print "<STR_LIT>" <EOL> while not vehicle . is_armable : <EOL> print "<STR_LIT>" <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> print "<STR_LIT>" <EOL> vehicle . mode = VehicleMode ( "<STR_LIT>" ) <EOL> vehicle . armed = True <EOL> while not vehicle . armed : <EOL> print "<STR_LIT>" <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> print "<STR_LIT>" <EOL> vehicle . simple_takeoff ( aTargetAltitude ) <EOL> while True : <EOL> print "<STR_LIT>" , vehicle . location . global_relative_frame . alt <EOL> if vehicle . location . global_relative_frame . alt >= aTargetAltitude * <NUM_LIT> : <EOL> print "<STR_LIT>" <EOL> break <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> arm_and_takeoff ( <NUM_LIT:5> ) <EOL> """<STR_LIT>""" <EOL> def condition_yaw ( heading , relative = False ) : <EOL> """<STR_LIT>""" <EOL> if relative : <EOL> is_relative = <NUM_LIT:1> <EOL> else : <EOL> is_relative = <NUM_LIT:0> <EOL> msg = vehicle . message_factory . command_long_encode ( <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_CMD_CONDITION_YAW , <EOL> <NUM_LIT:0> , <EOL> heading , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:1> , <EOL> is_relative , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> vehicle . send_mavlink ( msg ) <EOL> def set_roi ( location ) : <EOL> """<STR_LIT>""" <EOL> msg = vehicle . message_factory . command_long_encode ( <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_CMD_DO_SET_ROI , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> location . lat , <EOL> location . lon , <EOL> location . alt <EOL> ) <EOL> vehicle . send_mavlink ( msg ) <EOL> """<STR_LIT>""" <EOL> def get_location_metres ( original_location , dNorth , dEast ) : <EOL> """<STR_LIT>""" <EOL> earth_radius = <NUM_LIT> <EOL> dLat = dNorth / earth_radius <EOL> dLon = dEast / ( earth_radius * math . cos ( math . pi * original_location . lat / <NUM_LIT> ) ) <EOL> newlat = original_location . lat + ( dLat * <NUM_LIT> / math . pi ) <EOL> newlon = original_location . lon + ( dLon * <NUM_LIT> / math . pi ) <EOL> if type ( original_location ) is LocationGlobal : <EOL> targetlocation = LocationGlobal ( newlat , newlon , original_location . alt ) <EOL> elif type ( original_location ) is LocationGlobalRelative : <EOL> targetlocation = LocationGlobalRelative ( newlat , newlon , original_location . alt ) <EOL> else : <EOL> raise Exception ( "<STR_LIT>" ) <EOL> return targetlocation ; <EOL> def get_distance_metres ( aLocation1 , aLocation2 ) : <EOL> """<STR_LIT>""" <EOL> dlat = aLocation2 . lat - aLocation1 . lat <EOL> dlong = aLocation2 . lon - aLocation1 . lon <EOL> return math . sqrt ( ( dlat * dlat ) + ( dlong * dlong ) ) * <NUM_LIT> <EOL> def get_bearing ( aLocation1 , aLocation2 ) : <EOL> """<STR_LIT>""" <EOL> off_x = aLocation2 . lon - aLocation1 . lon <EOL> off_y = aLocation2 . lat - aLocation1 . lat <EOL> bearing = <NUM_LIT> + math . atan2 ( - off_y , off_x ) * <NUM_LIT> <EOL> if bearing < <NUM_LIT:0> : <EOL> bearing += <NUM_LIT> <EOL> return bearing ; <EOL> """<STR_LIT>""" <EOL> def goto_position_target_global_int ( aLocation ) : <EOL> """<STR_LIT>""" <EOL> msg = vehicle . message_factory . set_position_target_global_int_encode ( <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_FRAME_GLOBAL_RELATIVE_ALT_INT , <EOL> <NUM_LIT> , <EOL> aLocation . lat * <NUM_LIT> , <EOL> aLocation . lon * <NUM_LIT> , <EOL> aLocation . alt , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> vehicle . send_mavlink ( msg ) <EOL> def goto_position_target_local_ned ( north , east , down ) : <EOL> """<STR_LIT>""" <EOL> msg = vehicle . message_factory . set_position_target_local_ned_encode ( <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_FRAME_LOCAL_NED , <EOL> <NUM_LIT> , <EOL> north , east , down , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> vehicle . send_mavlink ( msg ) <EOL> def goto ( dNorth , dEast , gotoFunction = vehicle . simple_goto ) : <EOL> """<STR_LIT>""" <EOL> currentLocation = vehicle . location . global_relative_frame <EOL> targetLocation = get_location_metres ( currentLocation , dNorth , dEast ) <EOL> targetDistance = get_distance_metres ( currentLocation , targetLocation ) <EOL> gotoFunction ( targetLocation ) <EOL> while vehicle . mode . name == "<STR_LIT>" : <EOL> remainingDistance = get_distance_metres ( vehicle . location . global_relative_frame , targetLocation ) <EOL> print "<STR_LIT>" , remainingDistance <EOL> if remainingDistance <= targetDistance * <NUM_LIT> : <EOL> print "<STR_LIT>" <EOL> break ; <EOL> time . sleep ( <NUM_LIT:2> ) <EOL> """<STR_LIT>""" <EOL> def send_ned_velocity ( velocity_x , velocity_y , velocity_z , duration ) : <EOL> """<STR_LIT>""" <EOL> msg = vehicle . message_factory . set_position_target_local_ned_encode ( <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_FRAME_LOCAL_NED , <EOL> <NUM_LIT> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> velocity_x , velocity_y , velocity_z , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> for x in range ( <NUM_LIT:0> , duration ) : <EOL> vehicle . send_mavlink ( msg ) <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> def send_global_velocity ( velocity_x , velocity_y , velocity_z , duration ) : <EOL> """<STR_LIT>""" <EOL> msg = vehicle . message_factory . set_position_target_global_int_encode ( <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <EOL> mavutil . mavlink . MAV_FRAME_GLOBAL_RELATIVE_ALT_INT , <EOL> <NUM_LIT> , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <EOL> velocity_x , <EOL> velocity_y , <EOL> velocity_z , <EOL> <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <EOL> <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> for x in range ( <NUM_LIT:0> , duration ) : <EOL> vehicle . send_mavlink ( msg ) <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> """<STR_LIT>""" <EOL> print ( "<STR_LIT>" ) <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . groundspeed = <NUM_LIT:5> <EOL> print ( "<STR_LIT>" ) <EOL> goto ( <NUM_LIT> , - <NUM_LIT:50> ) <EOL> print ( "<STR_LIT>" ) <EOL> goto ( <NUM_LIT:0> , <NUM_LIT:100> ) <EOL> print ( "<STR_LIT>" ) <EOL> goto ( - <NUM_LIT> , - <NUM_LIT:50> ) <EOL> """<STR_LIT>""" <EOL> print ( "<STR_LIT>" ) <EOL> print ( "<STR_LIT>" ) <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . groundspeed = <NUM_LIT:5> <EOL> goto ( - <NUM_LIT:100> , - <NUM_LIT> , goto_position_target_global_int ) <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . groundspeed = <NUM_LIT:15> <EOL> print ( "<STR_LIT>" ) <EOL> goto ( <NUM_LIT:0> , <NUM_LIT> , goto_position_target_global_int ) <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . airspeed = <NUM_LIT:10> <EOL> print ( "<STR_LIT>" ) <EOL> goto ( <NUM_LIT:100> , - <NUM_LIT> , goto_position_target_global_int ) <EOL> """<STR_LIT>""" <EOL> print ( "<STR_LIT>" ) <EOL> DURATION = <NUM_LIT:20> <EOL> print ( "<STR_LIT>" % DURATION ) <EOL> goto_position_target_local_ned ( <NUM_LIT:50> , <NUM_LIT:0> , - <NUM_LIT:10> ) <EOL> print ( "<STR_LIT>" ) <EOL> set_roi ( vehicle . location . global_relative_frame ) <EOL> time . sleep ( DURATION ) <EOL> print ( "<STR_LIT>" ) <EOL> goto_position_target_local_ned ( <NUM_LIT:50> , <NUM_LIT:50> , - <NUM_LIT:10> ) <EOL> time . sleep ( DURATION ) <EOL> print ( "<STR_LIT>" ) <EOL> set_roi ( vehicle . location . global_relative_frame ) <EOL> print ( "<STR_LIT>" ) <EOL> goto_position_target_local_ned ( <NUM_LIT:0> , <NUM_LIT:50> , - <NUM_LIT:10> ) <EOL> time . sleep ( DURATION ) <EOL> print ( "<STR_LIT>" ) <EOL> goto_position_target_local_ned ( <NUM_LIT:0> , <NUM_LIT:0> , - <NUM_LIT:10> ) <EOL> time . sleep ( DURATION ) <EOL> """<STR_LIT>""" <EOL> NORTH = <NUM_LIT:2> <EOL> SOUTH = - <NUM_LIT:2> <EOL> EAST = <NUM_LIT:2> <EOL> WEST = - <NUM_LIT:2> <EOL> UP = - <NUM_LIT:0.5> <EOL> DOWN = <NUM_LIT:0.5> <EOL> print ( "<STR_LIT>" ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> ) <EOL> print ( "<STR_LIT>" ) <EOL> send_ned_velocity ( SOUTH , <NUM_LIT:0> , UP , DURATION ) <EOL> send_ned_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> ) <EOL> print ( "<STR_LIT>" ) <EOL> send_ned_velocity ( <NUM_LIT:0> , WEST , DOWN , DURATION ) <EOL> send_ned_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT:0> ) <EOL> print ( "<STR_LIT>" ) <EOL> send_ned_velocity ( NORTH , <NUM_LIT:0> , <NUM_LIT:0> , DURATION ) <EOL> send_ned_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> ) <EOL> print ( "<STR_LIT>" ) <EOL> send_ned_velocity ( <NUM_LIT:0> , EAST , <NUM_LIT:0> , DURATION ) <EOL> send_ned_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> """<STR_LIT>""" <EOL> print ( "<STR_LIT>" ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> ) <EOL> print ( "<STR_LIT>" ) <EOL> send_global_velocity ( SOUTH , WEST , UP , DURATION ) <EOL> send_global_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> , relative = True ) <EOL> print ( "<STR_LIT>" ) <EOL> send_global_velocity ( NORTH , WEST , DOWN , DURATION ) <EOL> send_global_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . home_location = vehicle . location . global_frame <EOL> print "<STR_LIT>" <EOL> cmds = vehicle . commands <EOL> cmds . download ( ) <EOL> cmds . wait_ready ( ) <EOL> print "<STR_LIT>" % vehicle . home_location <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> , relative = True ) <EOL> print ( "<STR_LIT>" ) <EOL> send_global_velocity ( NORTH , EAST , <NUM_LIT:0> , DURATION ) <EOL> send_global_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> print ( "<STR_LIT>" ) <EOL> condition_yaw ( <NUM_LIT> , relative = True ) <EOL> print ( "<STR_LIT>" ) <EOL> send_global_velocity ( SOUTH , EAST , <NUM_LIT:0> , DURATION ) <EOL> send_global_velocity ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) <EOL> """<STR_LIT>""" <EOL> print ( "<STR_LIT>" ) <EOL> vehicle . mode = VehicleMode ( "<STR_LIT>" ) <EOL> print "<STR_LIT>" <EOL> vehicle . close ( ) <EOL> if sitl is not None : <EOL> sitl . stop ( ) <EOL> print ( "<STR_LIT>" ) </s>
