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,