lab5_pt1_takeoff_land_new location

######## Program to take off the drone and land in new location:

from dronekit import connect, VehicleMode, LocationGlobalRelative
import time

# Connect to the vehicle
vehicle = connect('udp:127.0.0.1:14550')

# Arm and take off
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.simple_takeoff(10)

# Wait for the drone to reach a certain altitude
while True:
    altitude = vehicle.location.global_relative_frame.alt
    if altitude >= 9.5:  # target altitude - 0.5 meters
        break
    time.sleep(1)

# Move the drone to a new location
new_location = LocationGlobalRelative(37.793105, -122.398768, 20)
vehicle.simple_goto(new_location)

# Wait for the drone to reach the new location
while True:
    distance = vehicle.location.global_relative_frame.distance_to(new_location)
    if distance <= 1:  # target radius in meters
        break
    time.sleep(1)

# Land the drone
vehicle.mode = VehicleMode("LAND")

# Close the connection
vehicle.close()

#####################################################################
#####################################################################
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import time
import math

# Set up connection to vehicle
vehicle = connect('udp:127.0.0.1:14550')

# Set vehicle mode to GUIDED
vehicle.mode = VehicleMode("GUIDED")

# Define base location and target location
base_location = LocationGlobalRelative(37.6189, -122.3750, 10)
target_location = LocationGlobalRelative(37.6200, -122.3770, 20)

# Arm and takeoff
vehicle.armed = True
vehicle.simple_takeoff(base_location.alt)

# Wait for takeoff to complete
while True:
    if abs(vehicle.location.global_relative_frame.alt - base_location.alt) < 1.0:
        print("Reached target altitude")
        break
    time.sleep(1)

# Go to target location
vehicle.simple_goto(target_location)

def distance_to(self, other):
    dlat = other.lat - self.lat
    dlong = other.lon - self.lon
    return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5


# Wait for arrival at target location
while True:
    if distance_to(vehicle.location.global_relative_frame, target_location) < 1.0:
        print("Reached target location")
        break
    time.sleep(1)

# Set mode to RTL and land
vehicle.mode = VehicleMode("RTL")
while True:
    if vehicle.mode.name == "LAND":
        print("Vehicle landed")
        break
    time.sleep(1)

# Close connection to vehicle
vehicle.close()