def upload_trajectory(scf: SyncCrazyflie, data: Dict):
cf = scf.cf # type: Crazyflie
print('Starting upload')
trajectory_mem = scf.cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0]
TRAJECTORY_MAX_LENGTH = 31
trajectory = data['trajectory']
if len(trajectory) > TRAJECTORY_MAX_LENGTH:
raise ValueError("Trajectory too long for drone {:s}".format(
cf.link_uri))
for row in trajectory:
duration = row[0]
x = Poly4D.Poly(row[1:9])
y = Poly4D.Poly(row[9:17])
z = Poly4D.Poly(row[17:25])
yaw = Poly4D.Poly(row[25:33])
trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw))
print('Calling upload method')
uploader = Uploader(trajectory_mem)
uploader.upload()
print('Defining trajectory.')
cf.high_level_commander.define_trajectory(trajectory_id=1,
offset=0,
n_pieces=len(
trajectory_mem.poly4Ds))
def upload_trajectory(cf, trajectory_id, trajectory):
trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0]
total_duration = 0
for row in trajectory:
duration = row[0]
x = Poly4D.Poly(row[1:9])
y = Poly4D.Poly(row[9:17])
z = Poly4D.Poly(row[17:25])
yaw = Poly4D.Poly(row[25:33])
trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw))
total_duration += duration
Uploader().upload(trajectory_mem)
cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory))
return total_duration
def upload_trajectory(cf, trajectory_id, trajectory):
trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0]
total_duration = 0
for row in trajectory:
duration = row[0]
x = Poly4D.Poly(row[1:9])
y = Poly4D.Poly(row[9:17])
z = Poly4D.Poly(row[17:25])
yaw = Poly4D.Poly(row[25:33])
trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw))
total_duration += duration
upload_result = Uploader().upload(trajectory_mem)
if not upload_result:
print('Upload failed, aborting!')
sys.exit(1)
cf.high_level_commander.define_trajectory(trajectory_id, 0,
len(trajectory_mem.poly4Ds))
return total_duration
