import sensor, image, time, math, lcd

from pyb import UART
from pyb import LED
uart = UART(3, 115200)

LED(1).on()
LED(2).on()
LED(3).on()

lcd.init()

GRAYSCALE_THRESHOLD = [(0, 64)]

ROIS = [ # [ROI, weight]
        (0, 100, 160, 20, 0.7), # You'll need to tweak the weights for you app
        (0, 050, 160, 20, 0.3), # depending on how your robot is setup.
        (0, 000, 160, 20, 0.1)
       ]

weight_sum = 0 #权值和初始化
for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(30,time = 2000) # Let new settings take affect.
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking

clock = time.clock() # Tracks FPS.

def send_frame(cx, cy):
    uart.writechar(0xA5)
    uart.writechar(0x5A)
    uart.writechar(cx)
    uart.writechar(cy)
    uart.writechar(cx + cy)

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    centroid_sum = 0

    for r in ROIS:
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)
        # r[0:4] is roi tuple.

        if blobs:
            most_pixels = 0
            largest_blob = 0
            for i in range(len(blobs)):

                if blobs[i].pixels() > most_pixels:
                    most_pixels = blobs[i].pixels()
                    largest_blob = i
            img.draw_rectangle(blobs[largest_blob].rect())
            img.draw_rectangle((0,0,30, 30))
            #将此区域的像素数最大的颜色块画矩形和十字形标记出来
            img.draw_cross(blobs[largest_blob].cx(),
                           blobs[largest_blob].cy())

            centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight.


    center_pos = (centroid_sum / weight_sum) # Determine center of line.

    deflection_angle = 0

    deflection_angle = -math.atan((center_pos-80)/60)


    deflection_angle = math.degrees(deflection_angle)


    print("Turn Angle: %f" % deflection_angle)
    send_frame(blobs[largest_blob].cx(), blobs[largest_blob].cy())
    lcd.display(img)
    print(clock.fps()) # Note: Your OpenMV Cam runs about half as fast while

@3tiw 硬件连接如图 但是不知道怎么通讯 pixhawk怎么接收openmv的信息