def action(img_bytes, img_width, img_height, is_belly, ctrl_state,
vbat_flying_percentage, theta, phi, psi, altitude, vx, vy):
# Set up command defaults
zap = 0
phi = 0
theta = 0
gaz = 0
yaw = 0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
action.theta_1 = 0
# Create full-color image from bytes
image = cv.CreateImageHeader((img_width, img_height), cv.IPL_DEPTH_8U, 3)
cv.SetData(image, img_bytes, img_width * 3)
# Grab centroid of green ball
ctr = greenball_tracker.track(image)
# Use centroid if it exists
if ctr:
# Compute proportional distance (error) of centroid from image center
errx = _dst(ctr, 0, img_width)
erry = -_dst(ctr, 1, img_height)
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
pass
#phi = _pid(action.phi_1, errx, action.errx_1, Kpx, Kix, Kdx)
#gaz = _pid(action.gaz_1, erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.phi_1 = phi
action.gaz_1 = gaz
action.count += 1
theta = -0.125
else:
if action.count > 0:
theta = _pid(action.theta_1, vx, action.erry_1, Kpy, Kiy, Kdy)
action.theta_1 = theta
action.erry_1 = vx
action.count += 1
print vy
# Send control parameters back to drone
return (zap, phi, theta, gaz, yaw)
def action(img_bytes, img_width, img_height, is_belly, ctrl_state, vbat_flying_percentage, theta, phi, psi, altitude, vx, vy):
# Set up command defaults
zap = 0
phi = 0
theta = 0
gaz = 0
yaw = 0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
action.theta_1 = 0
# Create full-color image from bytes
image = cv.CreateImageHeader((img_width,img_height), cv.IPL_DEPTH_8U, 3)
cv.SetData(image, img_bytes, img_width*3)
# Grab centroid of green ball
ctr = greenball_tracker.track(image)
# Use centroid if it exists
if ctr:
# Compute proportional distance (error) of centroid from image center
errx = _dst(ctr, 0, img_width)
erry = -_dst(ctr, 1, img_height)
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
pass
#phi = _pid(action.phi_1, errx, action.errx_1, Kpx, Kix, Kdx)
#gaz = _pid(action.gaz_1, erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.phi_1 = phi
action.gaz_1 = gaz
action.count += 1
theta = -0.125
else:
if action.count > 0:
theta = _pid(action.theta_1, vx, action.erry_1, Kpy, Kiy, Kdy)
action.theta_1 = theta
action.erry_1 = vx
action.count += 1
print vy
# Send control parameters back to drone
return (zap, phi, theta, gaz, yaw)
def action(
img_bytes, img_width, img_height, is_belly, ctrl_state, vbat_flying_percentage, theta, phi, psi, altitude, vx, vy
):
# Set up command defaults
zap = 0
phi = 0
theta = 0
gaz = 0
yaw = 0
# Set up state variables first time around
if not hasattr(action, "count"):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
# Create full-color image from bytes
# Create full-color image from bytes
image = np.frombuffer(img_bytes, np.uint8)
image = np.ndarray.reshape(image, (img_height, img_width, 3))
# Grab centroid of green ball
ctr = greenball_tracker.track(image)
# Use centroid if it exists
if ctr:
# Compute proportional distance (error) of centroid from image center
errx = _dst(ctr, 0, img_width)
erry = -_dst(ctr, 1, img_height)
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
phi = _pid(action.phi_1, errx, action.errx_1, Kpx, Kix, Kdx)
gaz = _pid(action.gaz_1, erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.phi_1 = phi
action.gaz_1 = gaz
action.count += 1
# Send control parameters back to drone
return (zap, phi, theta, gaz, yaw)
def action(img_bytes, img_width, img_height, is_belly, ctrl_state,
vbat_flying_percentage, theta, phi, psi, altitude, vx, vy):
# Set up command defaults
zap = 0
phi = 0
theta = 0
gaz = 0
yaw = 0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
# Create full-color image from bytes
# Create full-color image from bytes
image = np.frombuffer(img_bytes, np.uint8)
image = np.ndarray.reshape(image, (img_height, img_width, 3))
# Grab centroid of green ball
ctr = greenball_tracker.track(image)
# Use centroid if it exists
if ctr:
# Compute proportional distance (error) of centroid from image center
errx = _dst(ctr, 0, img_width)
erry = -_dst(ctr, 1, img_height)
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
phi = _pid(action.phi_1, errx, action.errx_1, Kpx, Kix, Kdx)
gaz = _pid(action.gaz_1, erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.phi_1 = phi
action.gaz_1 = gaz
action.count += 1
# Send control parameters back to drone
return (zap, phi, theta, gaz, yaw)