def VariantVelocitySimulation(which_drone,
waypoints,
error_level=0.5,
time_step=0.01):
plt.ion()
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111)
#unlock the drone
ARM.Arm(which_drone)
if not which_drone.is_arm:
print('ERROR: the drone is locked!')
return
for k in range(1, len(waypoints)):
this_waypoint = waypoints[k]
#clean ax
ax.cla()
#start point this iteration
start_point = which_drone.position_NED
#create flight error
destination_point = Err.WayPointError(start_point, this_waypoint,
error_level)
#yaw in start point
start_yaw = Geom.Azimuth(start_point, destination_point)
#waypoint parameters
this_waypoint_params = list(destination_point -
start_point) + [start_yaw]
#Waypoint flight
WAYPOINT_FLIGHT.WaypointFlight(which_drone, this_waypoint_params, 1)
which_drone.Plot(ax, True, True)
plt.axis(FlightField(waypoints) * 1.1)
plt.title('Drone Flight Simulation 2D: Variant Velocity',
fontproperties=title_font)
plt.pause(time_step)
def UpdateFrame(index,
frame,
which_drone,
waypoints_3D,
ax,
error_level=0.5,
time_step=0.01):
try:
this_waypoint = waypoints_3D[index + 1]
#clean ax
ax.cla()
#start point this iteration
start_point = which_drone.position_NED
#create flight error
destination_point = Err.WayPointError(start_point, this_waypoint,
error_level)
#yaw in start point
start_yaw = Geom.Azimuth(start_point, destination_point)
#waypoint parameters
this_waypoint_params = list(destination_point -
start_point) + [start_yaw]
#Waypoint flight
WAYPOINT_FLIGHT.WaypointFlight(which_drone, this_waypoint_params, 1)
which_drone.Plot(ax, True, True)
plt.axis(Sim.FlightField(waypoints_3D) * 1.1)
plt.title('Drone Flight Simulation 2D: Variant Velocity',
fontproperties=title_font)
plt.pause(time_step)
return frame,
except IndexError:
return frame,
