I have attached my code below. Sorry unsure how to get the colours going
# LED Beacon Precison Landing
# Sends the drones position relative to a LED marker via Mavlink to the drones flight controller
# for autonomous precison landing.
import sensor, image, time, ulab, cmath, math, pyb, json
from ulab import numpy as np
from pyb import Timer
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE) # Setiing the camera to grayscale
sensor.set_framesize(sensor.QVGA) # Setting the cameras resolution (320 x 240)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # Must be turned off for tracking
sensor.set_auto_whitebal(False) # Must be turned off for tracking
clock = time.clock()
sensor.set_auto_exposure(False, exposure_us=150)
sensor.skip_frames(time = 1000)
start = pyb.millis() # Start timer
#----------------------------------Variables---------------------------------------------------------------------------
# LED configuration
X4 = np.array([0, 13.5, 0]).reshape((3,1)) # 3D fourth point coordinate - X=[x,y,z] LED 5
pts3d = np.array([[ 0, 13.5, 13.5], # [ LED_2_x, LED_3_x, LED_4_x],
[ 0, 0, 13.5], # [ LED_2_y, LED_3_y, LED_4_y],
[ 0, 0, 0]]) # [ 0, 0, 0]
Points= {} # Saved blob information {key : [x, y, LED number , start_time, stop_time, most_recent_time] }
# Confirmed LED numbers 2,3,4,5. 10 ->5, 8->4, 7->2, 6->3
# Left numbers are prerequisit LED name as period is checked twice
Old_Points = {} # Prev saved blob information
tot_points = 2 # Key for potential new LED point
pixel_threeshold = 50 # Acceptable distance away from prev postion
reset_point_timer = 250 # Deletes point after timer (ms)
pts2d =np.ones((3,3)) # 2D LED Coord matrix
x4 = np.ones((1,3)) # Initialising 2d point coordinate array Forth Point
'''
# Camera 1 Matrix
K = np.array([[777.14542236, 0, 310.20444678],
[ 0, 777.4151376, 243.61929354],
[ 0, 0, 1]])
'''
#Camera 2 Matrix
K = np.array([[777.06768, 0, 300.0053],
[ 0, 776.83673, 216.3983],
[ 0, 0, 1]])
send = 0
send_timer = 10000
buff_size = 10
center_x = 0
center_y = 0
blob5 = 1
blob2 = 1
thresholds = (250, 256) # Thresholds for bright white light from LED.
box_colour = (30, 150, 30)
medbuffer_x = np.zeros((1,10))
medbuffer_y = np.zeros((1,10))
medbuffer_z = np.zeros((1,10))
#--------------------------------Math Functions--------------------------------------------------------------------
# Dot product of two vector arrays
def dotProduct(vect_A, vect_B):
product = 0
n = 3
for i in range(0, n): # Loop to calculate the dot product
product = product + vect_A[i] * vect_B[i]
return product
# Calculates dot product of matrices
def dot_mat(mat1, mat2):
row1_num, col1_num = mat1.shape()
row2_num, col2_num = mat2.shape()
dot = np.zeros((row1_num, col2_num))
for row in range(0, row1_num):
current_row = mat1[row,:]
for col in range(0, col2_num):
current_col = mat2[:,col]
product = dotProduct(current_row, current_col)
dot[row, col] = product
return dot
# Calculates transpose of a matrix
def trans(Array):
transposed_array = []
for i in range(Array.shape()[1]):
new_row = list()
for sublist in Array:
new_row.append(sublist[i])
transposed_array.append(new_row)
return np.array(transposed_array)
# --------------------- Fourth Degree Polynoimal Solver -------------------
# Solves a quartic equation using Ferrari's method.
def Quartic(h):
A = h[0]
B = h[1]
C = h[2]
D = h[3]
E = h[4]
m_min = 1e-20
alpha = -3*B**2/(8*A**2) + C/A
beta = + B**3/(8*A**3) - B*C/(2*A**2) + D/A
gamma = -3*B**4/(256*A**4) + C*B**2/(16*A**3) - B*D/(4*A**2) +E/A
if beta == 0:
root1 = -B/(4*A) + cmath.sqrt( (-alpha + cmath.sqrt(alpha**2-4*gamma)) /2)
root2 = -B/(4*A) + cmath.sqrt( (-alpha - cmath.sqrt(alpha**2-4*gamma)) /2)
root3 = -B/(4*A) - cmath.sqrt( (-alpha + cmath.sqrt(alpha**2-4*gamma)) /2)
root4 = -B/(4*A) - cmath.sqrt( (-alpha - cmath.sqrt(alpha**2-4*gamma)) /2)
else:
P= -alpha**2/12 - gamma
Q= -alpha**3/108 + alpha*gamma/3 - beta**2/8
Rp= Q/2 + (Q**2/4 + P**3/27)**0.5
Rm= Q/2 - (Q**2/4 + P**3/27)**0.5
U=Rm**(1/3)
if(U==0):
y=-5/6*alpha - U
else:
y=-5/6*alpha - U + P/(3*U)
W=( alpha + 2*y )**(1/2)
root1 = -B/(4*A) + 0.5 * ( + W + ( -(3*alpha + 2*y + 2*beta/(W +m_min)) )**0.5 )
root2 = -B/(4*A) + 0.5 * ( - W + ( -(3*alpha + 2*y - 2*beta/(W+m_min)) )**0.5 )
root3 = -B/(4*A) + 0.5 * ( + W - ( -(3*alpha + 2*y + 2*beta/(W+m_min)) )**0.5 )
root4 = -B/(4*A) + 0.5 * ( - W - ( -(3*alpha + 2*y - 2*beta/(W+m_min)) )**0.5 )
return [root1, root2, root3, root4]
#------------------------------PnP Solver-----------------------------------------------------------
def pnp(X, pts2d, K):
m11 =(np.linalg.inv(K)).reshape((3,3))
m22 =(pts2d).reshape((3,3))
m = dot_mat(np.linalg.inv(K), pts2d)
m = m / (np.array([ np.linalg.norm(m[:,0]) , np.linalg.norm(m[:,1]) , np.linalg.norm(m[:,2]) ]))
# Permute m and X so that the longest distance is between X[:,0] and X[:,1]
d = ((np.array([X[:,1] - X[:,0],X[:,2] - X[:,1] ,X[:,0] - X[:,2]])).transpose()) ** 2
d = np.sqrt(d[0] + d[1] + d[2])
idx = np.argmax(d) # Finds index of maximum element in d array
if idx == 1:
X = trans(np.array([ X[:,1] , X[:,2], X[:,0] ]))
m = trans(np.array([ m[:,1] , m[:,2], m[:,0] ]))
elif idx ==2:
X = trans(np.array([ X[:,0] , X[:,2], X[:,1] ]))
m = trans(np.array([ m[:,0] , m[:,2], m[:,1] ]))
X21 = X[:,1] - X[:,0]
X31 = X[:,2] - X[:,0]
nx = X21
nx = nx / np.linalg.norm(nx)
nz = np.cross(nx, X31)
nz = nz / np.linalg.norm(nz)
ny = np.cross(nz, nx)
N = trans(np.array([nx, ny, nz]))
# Calculate coefficients of the polinomial for solving projective depths
a = dot_mat( (np.array((N[:,0]))).reshape((1,3)) , X21.reshape((3,1)) )
b = dot_mat( (np.array((N[:,0]))).reshape((1,3)) , X31.reshape((3,1)) )
c = dot_mat( (np.array((N[:,1]))).reshape((1,3)) , X31.reshape((3,1)) )
M12 = dot_mat( (np.array((m[:,0]))).reshape((1,3)) , (np.array((m[:,1]))).reshape((3,1)) )
M13 = dot_mat( (np.array((m[:,0]))).reshape((1,3)) , (np.array((m[:,2]))).reshape((3,1)) )
M23 = dot_mat( (np.array((m[:,1]))).reshape((1,3)) , (np.array((m[:,2]))).reshape((3,1)) )
p = b/a
q = (b**2 + c**2) / (a**2)
f = np.array( [p[0][0], -M23[0][0], 0, -M12[0][0]*(2*p[0][0]-1), M13[0][0], p[0][0]-1] )
g = np.array( [q[0][0], 0, -1, -2*M12[0][0]*q[0][0], 2*M13[0][0], q[0][0]-1] )
h1 = -(f[0]**2) + g[0]*(f[1]**2)
h2 = (f[1]**2)*g[3] - 2*f[0]*f[3] - 2*f[0]*f[1]*f[4] + 2*f[1]*f[4]*g[0]
h3 = (f[4]**2)*g[0] - 2*f[0]*(f[4]**2) - 2*f[0]*f[5] + (f[1]**2)*g[5] - (f[3]**2) - 2*f[1]*f[3]*f[4] + 2*f[1]*f[4]*g[3]
h4 = (f[4]**2)*g[3] - 2*f[3]*(f[4]**2) - 2*f[3]*f[5] - 2*f[1]*f[4]*f[5] + 2*f[1]*f[4]*g[5]
h5 = - 2*f[5]*(f[4]**2) + g[5]*(f[4]**2) - (f[5]**2)
h = np.array([h1, h2, h3, h4, h5])
x = Quartic(h) # Finds the roots of fourth degree polynomial
# Discards complex results
x_new = []
for i in range(4):
if isinstance(x[i], complex):
if ((abs((x[i].imag)))<1e-1) and (x[i].real>0):
x_new.append(x[i].real)
else:
if (x[i]>0):
x_new.append(x[i])
x = np.array(x_new)
x = np.array([1.0147, 0.9929])
y = -np.array(((f[0]*x+f[3] )*x + f[5]) / (f[4] + f[1]*x))
# Recovers motion
nsols = len(x)
A = m * np.array([-1, 1, 0])
B = m * np.array([-1, 0, 1])
C = B- p*A
R = [np.zeros((3,3)), np.zeros((3,3)), np.zeros((3,3)), np.zeros((3,3))]
t = np.zeros((3,nsols))
for i in range(0, nsols):
lambda1 = (np.array([1, x[i], y[i]])).reshape((3,1))
s = np.linalg.norm(dot_mat( A, lambda1 )) / a
d = (lambda1/s).reshape((3,1))
r1 = dot_mat(A,d) /a
r2 = dot_mat(C,d) /c
r31 = r1[1][0]* r2[2][0] - (r1[2][0]) * (r2[1][0])
r32 = r1[2][0]* r2[0][0] - (r1[0][0]) * (r2[2][0])
r33 = r1[0][0]* r2[1][0] - (r1[1][0]) * (r2[0][0])
r3 = np.array([r31, r32, r33])
Rc = trans(np.array([ [r1[0][0], r1[1][0], r1[2][0]] , [r2[0][0], r2[1][0], r2[2][0]] , [r3[0], r3[1], r3[2]] ]))
tc = (d[0]*m[:,0])
tc[2] = (0.496283567*tc[2]) - 6.2002684 # Scales z into centimeters
tc[0], tc[1] = scaling(tc[2], tc[0], tc[1]) # Scales x and y into centimeters based on the z position
R[i] = dot_mat(Rc, trans(N)) # Rotation matrix
t[:,i] = tc # Adds new translation to the translation matrix
return R, t
#--------------------------------------Selects most plausible solution ---------------------------------------------
# A fourth point is used to find the best solution by selecting the solution with the minimal error
def best_solution(R, t, X4, m4):
min_error = float('inf')
min_idx = []
for ii in range(t.shape()[1]):
m2 = dot_mat((R[ii].reshape((3,3))), (X4)) + (t[:,ii].reshape((3,1)))
m2 = m2 / m2[2]
error = np.linalg.norm(m4 - m2) # Compares predicted fourth point pixel position to the actual position
if error < min_error:
min_error = error
min_idx = ii
return R[min_idx], t[:, min_idx]
# ------------------------MAVLINK-----------------------------------------------------------------------
uart_baudrate = 115200
MAV_system_id = 1
MAV_component_id = 0x54
MAX_DISTANCE_SENSOR_enable = True
lens_mm = 2.8 # Standard Lens.
lens_to_camera_mm = 22 # Standard Lens.
sensor_w_mm = 3.984 # For OV7725 sensor - see datasheet.
sensor_h_mm = 2.952 # For OV7725 sensor - see datasheet.
x_res = 320 # QVGA
y_res = 240 # QVGA
f_x = (lens_mm / sensor_w_mm) * x_res
f_y = (lens_mm / sensor_h_mm) * y_res
c_x = x_res / 2
c_y = y_res / 2
h_fov = 2 * math.atan((sensor_w_mm / 2) / lens_mm)
v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm)
# Link Setup
uart = pyb.UART(3, uart_baudrate, timeout_char = 1000)
# Helper Stuff
packet_sequence = 0
def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master/checksum.h
output = 0xFFFF
for i in range(len(data)):
tmp = data[i] ^ (output & 0xFF)
tmp = (tmp ^ (tmp << 4)) & 0xFF
output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
tmp = extra ^ (output & 0xFF)
tmp = (tmp ^ (tmp << 4)) & 0xFF
output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
return output
MAV_DISTANCE_SENSOR_message_id = 132 # Mavlink message number
MAV_DISTANCE_SENSOR_min_distance = 1 # in cm
MAV_DISTANCE_SENSOR_max_distance = 10000 # in cm
MAV_DISTANCE_SENSOR_type = 0 # MAV_DISTANCE_SENSOR_LASER
MAV_DISTANCE_SENSOR_id = 0 # unused
MAV_DISTANCE_SENSOR_orientation = 25 # MAV_SENSOR_ROTATION_PITCH_270
MAV_DISTANCE_SENSOR_covariance = 0 # unused
MAV_DISTANCE_SENSOR_extra_crc = 85
def send_distance_sensor_packet(t_best):
global packet_sequence
temp = struct.pack("<lhhhbbbb",
0,
MAV_DISTANCE_SENSOR_min_distance,
MAV_DISTANCE_SENSOR_max_distance,
min(max(int(t_best[2]), MAV_DISTANCE_SENSOR_min_distance), MAV_DISTANCE_SENSOR_max_distance),
MAV_DISTANCE_SENSOR_type,
MAV_DISTANCE_SENSOR_id,
MAV_DISTANCE_SENSOR_orientation,
MAV_DISTANCE_SENSOR_covariance)
temp = struct.pack("<bbbbb14s",
14,
packet_sequence & 0xFF,
MAV_system_id,
MAV_component_id,
MAV_DISTANCE_SENSOR_message_id,
temp)
temp = struct.pack("<b19sh",
0xFE,
temp,
checksum(temp, MAV_DISTANCE_SENSOR_extra_crc))
packet_sequence += 1
uart.write(temp)
MAV_LANDING_TARGET_message_id = 149
MAV_LANDING_TARGET_min_distance = 1/100 # in meters
MAV_LANDING_TARGET_max_distance = 10000/100 # in meters
MAV_LANDING_TARGET_frame = 8 # MAV_FRAME_BODY_NED
MAV_LANDING_TARGET_extra_crc = 200
def send_landing_target_packet(t_best, w, h, center_x, center_y):
tot_distance = pythagoras( ( pythagoras(t_best[0], t_best[1])) , t_best[2])
global packet_sequence
temp = struct.pack("<qfffffbb",
0,
((center_x / w) - 0.5) * h_fov,
((center_y / h) - 0.5) * v_fov,
min(max(tot_distance/100 , MAV_LANDING_TARGET_min_distance), MAV_LANDING_TARGET_max_distance),
0.0,
0.0,
0,
MAV_LANDING_TARGET_frame)
temp = struct.pack("<bbbbb30s",
30,
packet_sequence & 0xFF,
MAV_system_id,
MAV_component_id,
MAV_LANDING_TARGET_message_id,
temp)
temp = struct.pack("<b35sh",
0xFE,
temp,
checksum(temp, MAV_LANDING_TARGET_extra_crc))
packet_sequence += 1
uart.write(temp)
# ----------------------------------Flickering LED tracking functions----------------------------------------------
# Changes y and x scaling factor depending on z position
def scaling(z_distance, x_value, y_value):
x_intercept = -0.41105*z_distance + 1.793
y_intercept = 0.316428814*z_distance + 0.45969444
x_scaled = x_intercept - x_value
y_scaled = y_intercept + y_value
return x_scaled, y_scaled
# Finds average of a list
def average(pixel_list):
return sum(pixel_list) / len(pixel_list)
def pythagoras(a, b):
return np.sqrt( (a)**2 + (b)**2 )
# Loops through previous saved points and see's if any of the new blobs meet the critera
# Updates positions of points to new blob postions if the blob coordinates are within an area threshold
def Updating_Points(Points, blob_list):
blob_found = np.zeros((1, len(blob_list)+1)) # Zero if not found, set to one if blob within point area threshold
for num in list(Points.keys()):
found = 0 # flag which is set to one if a blob meets critera
tot_error = float("inf")
counter = 0
for blob in blob_list:
counter = counter + 1
ratio = blob.w() / blob.h()
blobx = blob.cx()
bloby = blob.cy()
if (ratio >= 0.6) and (ratio <= 1.4):
img.draw_cross(blobx, bloby)
# Checks if the blobs coordinates are within the area threehold of the previous points coordinates
if (Points[num][0] + pixel_threeshold ) > blobx > (Points[num][0] - pixel_threeshold ) and (Points[num][1] + pixel_threeshold ) > bloby > (Points[num][1] - pixel_threeshold ):
error_dist = pythagoras(blobx - Points[num][0], bloby - Points[num][1]) # Finds pixel distance between previous point and new blob position
# Saves blob closet to previous saved point
if error_dist < tot_error:
tot_error = error_dist
best = blob
blob_counter = counter
found = 1
# Updates point coordinates to best saved blob position
if len(blob_list) > 0 and found == 1:
Points[num][0] = best.cx()
Points[num][1] = best.cy()
# Reset start and stop times
if Points[num][4] > Points[num][3]:
Points[num][3] = pyb.elapsed_millis(start)
Points[num][4] = 1
Points[num][5] = pyb.elapsed_millis(start)
found = 1
blob_found[0][blob_counter-1] = 1
return Points, blob_found
# Removes any duplicates and sets them to the value in Old_Points dictionary
def remove_duplicates(Points, Old):
temp = [] # List of x positions
temp2= [] # List of y positions
keys_saved = [] # List of keys
double_keys = [] # List of keys found to have the same x and y coordinates saved
res = dict() # New dictionary for LED points
# Finds duplicate points and adds them to double_keys list
for key, val in Points.items():
found_flag = 0
if len(temp2) > 0:
for index in range(len(temp)):
if val[0] == temp[index] and val[1] == temp2[index]:
found_flag = 1
# Check if postions if not alread found
if found_flag == 0:
temp.append(val[0])
temp2.append(val[1])
keys_saved.append(key)
res[key] = val
# Finds duplicate points and adds them to double array.
else:
for i in range(len(temp)):
if temp[i] == val[0] and temp2[i] == val[1]:
current = keys_saved[i]
if current not in double_keys:
double_keys.append(current)
if key not in double_keys:
double_keys.append(key)
dist_list = []
# Sort which position is right for each key in doubles list.
# The keys previous position in the old dicionary will be compared to the new position
# and the key with the smallest difference in distance will be set to this posiiton.
# The other duplicate keys will be set to their old position.
if len(double_keys) > 0:
new_point = Points[double_keys[0]]
for key_idx in range(len(double_keys)-1):
key = double_keys[key_idx]
current = double_keys[key_idx + 1]
# Compare distances of points
distance1 = pythagoras((Old[str(key)][0] - new_point[0]), (Old[str(key)][1] - new_point[1]))
distance2 = pythagoras((Old[str(current)][0] - new_point[0]), (Old[str(current)][1] - new_point[1]) )
if distance1 > distance2:
res[key] = Old[str(key)]
dist_list.append( (distance1, key) )
if distance2 < dist_list[0][0]:
res[dist_list[0][1]] = Old[str(dist_list[0][1])]
dist_list[0] = (distance2,current)
else:
res[current] = Old[str(current)]
else:
res[current] = Old[str(current)]
dist_list.append( (distance2, current) )
if distance1 < dist_list[0][0]:
res[dist_list[0][1]] = Old[str(dist_list[0][1])]
dist_list[0] = (distance1, key)
else:
res[key] = Old[str(key)]
key = dist_list[0][1]
res[dist_list[0][1]] = Old[str(dist_list[0][1])]
res[key][5] = pyb.elapsed_millis(start)
if Old[str(key)][3] == res[key][3]:
res[key][0] = Points[key][0]
res[key][1] = Points[key][1]
res[key][5] = pyb.elapsed_millis(start)
if res[key][4] > res[key][3]:
res[key][3] = pyb.elapsed_millis(start)
res[key][4] = 1
return res
# Determines if point is blinking at the correct period to be an LED
def LED_Finder(res, Old, pts2d, blob5, blob2, center_x, center_y):
flag_Detect = 4
flag_off = 0
Points3or4 = []
LED_x_pixels = []
LED_y_pixels = []
# Finds blobs period and determines if they are LEDs
for key_final, val_final in res.items():
send = 0
# Set stop time when stops detecting blob
if val_final[5] - pyb.elapsed_millis(start) < -50 and val_final[4] == 1:
res[key_final][4] = pyb.elapsed_millis(start)
# Compares blobs periods to wanted periods
if 1000 < (val_final[4] - val_final[3]) < 1150:
if res[key_final][2] == 1 or res[key_final][2] == 6 or res[key_final][2] == 10 or res[key_final][2] == 8:
res[key_final][2] = 7
res[key_final][4] = 99999999
elif res[key_final][2] == 7 and blob2 == 1:
res[key_final][2] = 2
blob5 = 0
elif res[key_final][2] == 7:
res[key_final][2] = 2
flag_off = flag_off +1
flag = 0
if 850 < (val_final[4] - val_final[3]) < 1000:
Points3or4.append(key_final)
flag = 1
flag_off = flag_off +1
elif 700 < (val_final[4] - val_final[3]) < 850:
if res[key_final][2] == 1 or res[key_final][2] == 6 or res[key_final][2] == 8 or res[key_final][2] == 7: # needs to see period twice before confirmed
res[key_final][2] = 10
res[key_final][4] = 99999999
elif res[key_final][2] == 10 and blob5 ==1:
res[key_final][2] = 5
blob5 = 0
elif res[key_final][2] == 10:
res[key_final][2] = 2
flag_off = flag_off +1
# Adds LED positions to pts2d matrix for pnp
if val_final[2] == 2:
pts2d[0,0] = val_final[0]
pts2d[1,0] = val_final[1]
LED_x_pixels.append(val_final[0])
LED_y_pixels.append(val_final[1])
img.draw_cross(val_final[0], val_final[1], (40, 150, 40))
img.draw_circle(val_final[0], val_final[1], 4, (40, 150, 40))
flag_Detect = flag_Detect - 1
elif val_final[2] == 3:
pts2d[0,1] = val_final[0]
pts2d[1,1] = val_final[1]
LED_x_pixels.append(val_final[0])
LED_y_pixels.append(val_final[1])
img.draw_cross(val_final[0], val_final[1], (40, 150, 40))
flag_Detect = flag_Detect -1
elif val_final[2] == 4:
LED_x_pixels.append(val_final[0])
LED_y_pixels.append(val_final[1])
pts2d[0,2] = val_final[0]
pts2d[1,2] = val_final[1]
img.draw_cross(val_final[0], val_final[1], (40, 150, 40))
flag_Detect = flag_Detect -1
elif val_final[2] == 5:
LED_x_pixels.append(val_final[0])
LED_y_pixels.append(val_final[1])
x4[0][0] = val_final[0]
x4[0][1] = val_final[1]
img.draw_circle(val_final[0], val_final[1], 3, (40, 150, 40))
flag_Detect = flag_Detect -1
if flag_Detect == 0 and flag_off ==0:
send = 1 # If 1 send LED info to pnp
center_x = average(LED_x_pixels) # Average x pixel of beacon
center_y = average(LED_y_pixels) # Average y pixel of beacon
# Removes points after it has not been detected for a certain amount of time or near edge of image
if (val_final[5] - pyb.elapsed_millis(start) < -reset_point_timer) and val_final[2] != 1:
res.clear()
Points3or4.clear()
blob5 = 1
blob2 = 1
elif (val_final[5] - pyb.elapsed_millis(start) < -reset_point_timer) or (2 > val_final[0] > 318) or (2 > val_final[1] > 238):
del res[key_final]
if flag == 1:
del Points3or4[-1]
# Sorts between LED 4 and 3 which flicker at the same frequency
if len(Points3or4) == 2 and (pts2d[0,0] != 1 or pts2d[1,0] != 1) :
pt1 = Points3or4[0]
distance1 = pythagoras( (res[pt1][0] - pts2d[0,0]), (res[pt1][1] - pts2d[1,0]))
distance2 = pythagoras( (res[Points3or4[1]][0] - pts2d[0,0]) , (res[Points3or4[1]][1] - pts2d[1,0]) )
if distance1 > distance2:
if res[Points3or4[0]][2] == 1 or res[Points3or4[0]][2] == 6 or res[Points3or4[0]][2] == 10 or res[Points3or4[0]][2] == 7:
res[Points3or4[0]][2] = 8
res[Points3or4[0]][4] = 99999999
elif res[Points3or4[0]][2] == 8:
res[Points3or4[0]][2] = 4
pts2d[0,1] = res[Points3or4[0]][0]
pts2d[1,1] = res[Points3or4[0]][1]
if res[Points3or4[1]][2] == 1 or res[Points3or4[1]][2] == 8 or res[Points3or4[1]][2] == 10 or res[Points3or4[1]][2] == 7:
res[Points3or4[1]][2] = 6
res[Points3or4[1]][4] = 99999999
elif res[Points3or4[1]][2] == 6:
res[Points3or4[1]][2] = 3
pts2d[0,1] = res[Points3or4[1]][0]
pts2d[1,1] = res[Points3or4[1]][1]
else:
if res[Points3or4[0]][2] == 1 or res[Points3or4[0]][2] == 8 or res[Points3or4[0]][2] == 10 or res[Points3or4[0]][2] == 7:
res[Points3or4[0]][2] = 6
res[Points3or4[0]][4] = 99999999
elif res[Points3or4[0]][2] == 6:
res[Points3or4[0]][2] = 3
if res[Points3or4[1]][2] == 1 or res[Points3or4[1]][2] == 6 or res[Points3or4[1]][2] == 10 or res[Points3or4[1]][2] == 7:
res[Points3or4[1]][2] = 8
res[Points3or4[1]][4] = 99999999
elif res[Points3or4[1]][2] == 8:
res[Points3or4[1]][2] = 4
return res, send, pts2d, blob5, blob2, center_x, center_y
# Adds new points if not already in point dictionary
def new_point(blob_found, blob_list, Points, tot_points):
if len(blob_list) > 0:
for i in range(len(blob_found[0])-1):
blob = blob_list[i]
ratio = blob.w() / blob.h()
# Checks if the blob has been added already and if it fits the critera
if blob_found[0][i] == 0 and (ratio >= 0.6) and (ratio <= 1.4):
if len(Points) > 0:
found1 = 0
for ii in Points.keys():
if blob.cx() == Points[ii][0] and blob.cy() == Points[ii][1]:
found1 = 1
Points[ii][3] = pyb.elapsed_millis(start)
Points[ii][4] = 1
Points[ii][5] = pyb.elapsed_millis(start)
if len(Points) == 0 or found1 == 0:
Points[tot_points] = [ blob.cx(), blob.cy(), 1, pyb.elapsed_millis(start), 1, pyb.elapsed_millis(start)] # Adding new point
# Increase total key count
if tot_points < 30:
tot_points = tot_points +1
else:
tot_points = 1
return Points, tot_points
#-----------------------------------------Main Loop----------------------------------------------------------------------------
while(True):
clock.tick()
img = sensor.snapshot().lens_corr(strength = 1.15, zoom = 1.0) # Lens correction to fix lens distortion in an image
blob_list = img.find_blobs([thresholds], pixels_threshold=10, area_threshold=10, merge=True) # List of all blobs of white pixels larger than the set thresholds
# print("Compactness:", blob.compactness())
# print("Roundness:", blob.roundness())
# print("Area Threshold:", blob.area())
# print("Pixel Threshold:", blob.pixels())
# Updates positions of points to new blob postions if the blob coordinates are within an area threshold
Points, blob_found = Updating_Points(Points, blob_list)
if len(Points) > 0:
Points = remove_duplicates(Points, Old_Points) # Removes any duplicates and sets them to the value in Old_Points dictionary
# Determines if point is blinking at the correct period to be an LED
Points, send, pts2d, blob5, blob2, center_x, center_y = LED_Finder(Points, Old_Points, pts2d, blob5, blob2, center_x, center_y)
Points, tot_points = new_point(blob_found, blob_list, Points, tot_points) # Adds new points if not already in point dictionary
if send == 1:
R, t = pnp(pts3d, pts2d, K) # Finds rotation and transtion of camera relative to marker
R_best, t_best = best_solution(R, t, X4, x4) # A foruth point is used to find the best solution by selecting the solution with the minimal error
# send_distance_sensor_packet(t_best) # Mavlink Message
# send_landing_target_packet(t_best, img.width(), img.height(),center_x ,center_y) # Mavlink Message
print(t)
print(t_best)
print(' ')
print('--------------')
Old_Points = json.loads(json.dumps(Points)) # Saves current points into an old points dictionary
The openMV is connected to the computer via usb.