for i in range(nrow):
# For all the waypoints recoreded, convert them from local NED w.r.t. the heading of the drone to global NED (rotating axes w.r.t. yaw angle).
list_location.append(drone.local_NED_to_global_NED(*LocationTuples[i]), home_yaw)
print('Coordinates in local NED of the waypoints (starting at path planning origin):')
for row in list_location:
print(row)
# print('Coordinates in WGS84 of the waypoints:')
# for row in list_location:
# print(drone.global_NED_to_wp(drone.vehicle.location.global_relative_frame, row[0], row[1], row[2]))
for i in range(points_to_cover):
# Given the global NED waypoints w.r.t. the home location (EKF origin), navigate the drone by specifying the frame.
print('Goto ({}, {}, {})'.format(list_location[i]))
drone.goto_local_NED(list_location[i][0], list_location[i][1], list_location[i][2], frame)
# if i == 0:
# drone.goto_global_NED(*list_location[i])
# else:
# drone.goto_global_NED(list_location[i][0] - list_location[i-1][0], list_location[i][1] - list_location[i-1][1], list_location[i][2] - list_location[i-1][2])
# Update the distance of the current position of the drone to the hoop. (1m is the distance of the drone from the hoop originally).
distance_to_hoop = 1 - drone.distance_covered_along_track(home_yaw)
# When the vision cannot be used anymore, continue the navigation of the most recently computed path.
for i in range(points_to_cover, len(list_location)):
print('Goto ({}, {}, {})'.format(list_location[i]))
drone.goto_local_NED(list_location[i][0], list_location[i][1], list_location[i][2], frame)
# for i in range(len(list_velocity)):
# print('Start Mission %d' % (i + 1))
# drone.set_velocity_local_NED(*list_velocity[i], 1)
description=
'Print out vehicle state information. Connects to SITL on local PC by default.'
)
parser.add_argument(
'--connect',
help=
"vehicle connection target string. If not specified, SITL automatically started and used."
)
args = parser.parse_args()
connection_string = args.connect
sitl = None
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
drone = Cyclone(connection_string, configs)
# drone.awake_script()
drone.arm_and_takeoff(5)
drone.set_airspeed(5)
print(drone.vehicle.location.local_frame)
# drone.goto_global_NED(0, 5, 0)
drone.goto_local_NED(5, 0, 0, mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED)
# drone.goto_local_NED(0, 5, -5, mavutil.mavlink.MAV_FRAME_LOCAL_NED)
# drone.set_position_target_local_NED(0, 5, -5, mavutil.mavlink.MAV_FRAME_LOCAL_NED)
# time.sleep(10)
print(drone.vehicle.location.local_frame)
del drone
drone.set_airspeed(5)
startPosition = drone.vehicle.location.local_frame
ang = drone.vehicle.attitude
print("Roll, Pitch, Yaw: {}, {}, {}".format(ang.roll, ang.pitch, ang.yaw))
print("Initialized")
#drone.goto_global_NED(0, 5, 0)
point1 = drone.translate_location(
drone.rotate_location(LocationLocal(5, 0, -5), Attitude(0, ang.yaw, 0)),
startPosition)
point2 = drone.translate_location(
drone.rotate_location(LocationLocal(5, 5, -5), Attitude(0, ang.yaw, 0)),
startPosition)
point3 = drone.translate_location(
drone.rotate_location(LocationLocal(5, 5, 0), Attitude(0, ang.yaw, 0)),
startPosition)
print("Point1: {}, {}, {}".format(point1.north, point1.east, point1.down))
print("Point2: {}, {}, {}".format(point2.north, point2.east, point2.down))
print("Point3: {}, {}, {}".format(point3.north, point3.east, point3.down))
drone.goto_local_NED(point1.north, point1.east, point1.down,
mavutil.mavlink.MAV_FRAME_LOCAL_NED)
print(drone.vehicle.location.local_frame)
drone.goto_local_NED(point2.north, point2.east, point2.down,
mavutil.mavlink.MAV_FRAME_LOCAL_NED)
print(drone.vehicle.location.local_frame)
drone.goto_local_NED(point3.north, point3.east, point3.down,
mavutil.mavlink.MAV_FRAME_LOCAL_NED)
print(drone.vehicle.location.local_frame)
drone.obtain_home_location()
del drone