I'm an electrical engineering student doing my final year thesis on visual control systems. I have connected an onboard camera on the crazyflie and my goal is to have it yaw towards a specific target, go to it and land. So far, it seems to be working fine, but I have a problem with the logging API in Python. I'm using the synchronous logging scheme and for some reason, after some time (usually 15-30 sec) the logging stops and the whole system becomes unresponsive. Here's me running the program: https://www.youtube.com/watch?v=a90CCFLT794
Here's part of the main loop:
Code: Select all
with SyncCrazyflie(URI) as scf:
# Define motion commander to crazyflie as mc:
cf2 = CF(scf, available)
cf2_psi = cf2.psi # Get current yaw-angle of cf
cf2_bat = cf2.vbat # Get current battery level of cf
marker_psi = 0 # Set marker angle to zero initially
pid = PID(0, 0, 0, setpoint=marker_psi) # Initialize PID
# Define pid parameters
pid.tunings = (Kp, Kd, Ki)
pid.output_limits = (-60, 60)
pid.proportional_on_measurment = False
if cf2_bat >= low_battery:
cf2.takeoff() # CF takes off from ground
print("Taking off ! Battery level: " + str(cf2_bat))
time.sleep(1.5) # Let the CF hover for 1.5s
t_init = time.time()
endTime = t_init + 30 # Let the CF do its thing for some time
# Add values to array for logging/plotting
setpoint = np.append(setpoint, marker_psi) # Commanded yaw
yaw_cf = np.append(yaw_cf, cf2.psi) # Actual yaw of CF
ar_time = np.append(ar_time, 0) # Time vector
ar_bat = np.append(ar_bat, cf2_bat)
ar_cf2_yaw_corr = np.append(ar_cf2_yaw_corr, marker_psi-yaw_cf)
A = scf.is_link_open()
while time.time() < endTime and cf2_bat >= low_battery and A:
# Get angle of marker
try:
# Get marker yaw-angle, img and translation vector(xyz)
marker_psi, img, t_vec = getMarkerAngles(cam)
# Get translational distance in Z-dir, marker->lens
if t_vec is not None:
try:
dist_z = np.asarray(t_vec).tolist()
dist_z = np.reshape(dist_z, [1, 3])
dist_z = dist_z[0, 2]
except ValueError:
print(err_dict["1"])
else:
dist_z = None
# Moving average filter
if marker_psi is not None:
zs = np.append(zs, marker_psi)
ds = np.append(ds, dist_z)
marker_psi = np.average(zs)
dist_z = np.average(ds)
if np.size(zs) >= samples_max:
zs = np.delete(zs, 0)
ds = np.delete(ds, 0)
# perform kalman filtering
f.update(marker_psi) # yaw
f.predict()
d.update(dist_z) # transl_z
d.predict()
# Set an inertial degregation to velocity for each iteration
if marker_psi is None and nf < nf_max and None not in (f.x[0], d.x[0], cf2_psi):
# f.x and d.x holds the estimated values
f.x[1] *= alpha
d.x[1] *= alpha
yaw_cmd = f.x[0, 0] # commanded yaw
cf2_yaw_corr = -cf2_psi #- f.x[0, 0] # compensated yaw
transl_z_cmd = round(d.x[0, 0]) # commanded transl_z
nf += 1
elif marker_psi is not None and marker_psi != 0:
nf = 0
yaw_cmd = marker_psi # commanded yaw
cf2_yaw_corr = -cf2_psi #- marker_psi # compensated yaw
transl_z_cmd = round(dist_z) # commanded transl_z
else:
yaw_cmd = 0
print("Marker not found")
except (cv2.error, TypeError, AttributeError):
print(err_dict["3"])
print("Error: ", sys.exc_info()[0])
cam.release()
cv2.destroyAllWindows()
cf2.land() # Land the CF2
print(TypeError)
traceback.print_exc()
break
pid.setpoint = yaw_cmd # Assign setpoint to PID-ctrl
dt = time.time() - t_init # compute elapsed time
# print text on image
cv2.putText(img, "t: "+str(round(dt, 2)),
timestampPosition,
font,
fontScale,
fontColor,
lineType)
cv2.putText(img, "psi: "+str(round(yaw_cmd, 2)),
yawstampPosition,
font,
fontScale,
fontColor,
lineType)
if dispLiveVid:
cv2.imshow('image', img) # display current frame
if cv2.waitKey(25) & 0xFF == ord('q'):
cf2.land()
cv2.destroyAllWindows()
break
out.write(img) # write img frame to video file
# Send command signal to CF2 and update cf2_psi
# 'psi' = yaw
# 'theta' = pitch
# 'phi' = roll
# 'z' = forwards/backwards
# ** Send signal to CF + get battery level and yaw-angle
try:
cf2_psi, cf2_bat = cf2.update(pid.setpoint, 'psi', turnRate)
The update function calls the synchronous logging:
Code: Select all
def getData(scf, available):
lg_stab = LogConfig(name='Stabilizer', period_in_ms=12)
# lg_stab.add_variable('stabilizer.roll', 'float')
lg_stab.add_variable('pm.vbat', 'float')
lg_stab.add_variable('stabilizer.yaw', 'float')
with SyncLogger(scf, lg_stab) as logger:
for log_entry in logger:
# timestamp = log_entry[0]
data = log_entry[1]
# logconf_name = logger[2]
y = data.get('stabilizer.yaw')
vbat = data.get('pm.vbat')
return(y, vbat)
Thanks!
EDIT: I solved the issue, the problem was that I call the
Code: Select all
with SyncLogger(scf, lg_stab) as logger:
for log_entry in logger: