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)