Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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,