cmds.download()
cmds.wait_ready()
drone.home_location = drone.location.global_frame
time.sleep(0.1)
home=drone.home_location
adds_3wayP_mission(drone, home, drone.heading, HEIGHT, scale=scale, windy=windy)
#adds_10wayP_mission(drone, home, drone.heading, HEIGHT, scale=scale, windy=windy)
# initialize logger
time_string = str(datetime.datetime.now()).replace(':', '-').replace(' ', '_').split('.')[0]
fname = 'log_sim/' + time_string + '.log'
logging.basicConfig(filename=fname,level=logging.DEBUG)
# initialize CX models
cx_gps = cx_rate.CXRate(noise = 0)
tb1_gps = np.zeros(central_complex.N_TB1)
memory_gps = 0.5 * np.ones(central_complex.N_CPU4)
cpu4_gps = np.zeros(16)
# takeoff and set to mission mode.
state = arm_and_takeoff(drone, HEIGHT)
drone.mode = VehicleMode("AUTO")
while drone.mode.name != "AUTO":
print "Pre-intialisation, waiting for the AUTO mode."
time.sleep(2)
# wait until reach first waypoint, 1->home, 2->takeoff
nextwaypoint = drone.commands.next
while nextwaypoint <= 1:
print "Initialisation, Moving to waypoint", drone.commands.next+1
nextwaypoint = drone.commands.next
adds_10wayP_mission(drone,
home,
drone.heading,
HEIGHT,
scale=scale,
windy=windy)
# initialize logger
time_string = str(datetime.datetime.now()).replace(':', '-').replace(
' ', '_').split('.')[0]
fname = 'log_homing/' + time_string + '.log'
logging.basicConfig(filename=fname, level=logging.DEBUG)
logging.info("Resolotion:{},{}".format(resolution[0], resolution[1]))
# initialize CX models
cx_optical = cx_rate.CXRate(noise=0)
tb1_optical = np.zeros(central_complex.N_TB1)
memory_optical = 0.5 * np.ones(central_complex.N_CPU4)
cx_gps = cx_rate.CXRate(noise=0)
tb1_gps = np.zeros(central_complex.N_TB1)
memory_gps = 0.5 * np.ones(central_complex.N_CPU4)
cpu4_gps = np.zeros(16)
# initialize camera and optical flow
frame_num = 0
picam = picameraThread(1, "picamera_video", resolution, 30)
picam.start()
fw, fh = resolution
time.sleep(0.1) # allow the camera to warmup
print("Frame size: {}*{}".format(fw, fh))
import numpy as np
import cv2
import sys, os, time
import dronekit
import matplotlib
import matplotlib.pyplot as plt
from CX_model import cx_rate, central_complex
from CX_model.optical_flow import Optical_flow, FRAME_DIM
from CX_model.central_complex import update_cells
from CX_model.drone_basic import arm, arm_and_takeoff
from CX_model.graphics import draw_flow
# initialize CX model
cx = cx_rate.CXRate(noise = 0)
tb1 = np.zeros(central_complex.N_TB1)
memory = 0.5 * np.ones(central_complex.N_CPU4)
# initialize camera
frame_num = 0
cap = cv2.VideoCapture(sys.argv[1])
for i in range(1):
ret, frame1 = cap.read() # Skip frames
frame_num += 1
temp = cv2.cvtColor(frame1,cv2.COLOR_BGR2GRAY)
dim = temp.shape[::-1]
# intialise optical flow object
optflow = Optical_flow((312,286));
prvs = temp #optflow.undistort(temp)
(fh, fw) = prvs.shape
print("Frame size: {0}*{1}".format(fw,fh))