lab5_pt2_simulateMission_usingWaypoints

######### Program to simulate a mission using series of waypoints
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)

# Define the mission waypoints
waypoints = [
    LocationGlobalRelative(37.793105, -122.398768, 20),
    LocationGlobalRelative(37.793109, -122.398824, 20),
    LocationGlobalRelative(37.793095, -122.398857, 20),
    LocationGlobalRelative(37.793057, -122.398843, 20),
    LocationGlobalRelative(37.793042, -122.398797, 20),
    LocationGlobalRelative(37.793050, -122.398751, 20),
    LocationGlobalRelative(37.793084, -122.398722, 20),
    LocationGlobalRelative(37.793119, -122.398724, 20)
]

# Fly the mission
for wp in waypoints:
    vehicle.simple_goto(wp)
    while True:
        distance = vehicle.location.global_relative_frame.distance_to(wp)
        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
import time, math

vehicle = connect('udp:127.0.0.1:14550')

# prep for liftoff
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.simple_takeoff(10)

# let the drone cook
while True:
    altitude = vehicle.location.global_relative_frame.alt
    if altitude >= 9.5:  # target altitude - 0.5 meters
        break
    time.sleep(1)

# points to be crossed during flight
waypoints = [
    LocationGlobalRelative(37.6210, -122.3880, 20),
    LocationGlobalRelative(37.6225, -122.3860, 20),
    LocationGlobalRelative(37.6235, -122.3840, 20),
    LocationGlobalRelative(37.6245, -122.3820, 20),
    LocationGlobalRelative(37.6255, -122.3800, 20),
    LocationGlobalRelative(37.6265, -122.3780, 20),
    LocationGlobalRelative(37.6275, -122.3760, 20),
    LocationGlobalRelative(37.6285, -122.3740, 20)
]

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


# LETSGOOO
for wp in waypoints:
    vehicle.simple_goto(wp)
    while True:
        if distance_to(vehicle.location.global_relative_frame, wp) <= 1.0:
            print("Reached target location")
            break
        time.sleep(1)

# land
vehicle.mode = VehicleMode("LAND")

vehicle.close()