mavlink_opticalflow.py

# هذا العمل مرخص بموجب ترخيص MIT.
# حقوق النشر (c) 2013-2023 OpenMV LLC. جميع الحقوق محفوظة.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# سكربت MAVLink للتدفق البصري (OpticalFlow).
#
# يقوم هذا السكربت بإرسال اكتشافات التدفق البصري (OpticalFlow) باستخدام بروتوكول MAVLink إلى
# وحدة تحكم ArduPilot/PixHawk للتحكم في الموضع باستخدام كاميرا OpenMV Cam الخاصة بك.
#
# P4 = TXD

import csi
import image
import struct
import time
import machine

UART_BAUDRATE = 115200
MAV_system_id = 1
MAV_component_id = 0x54
packet_sequence = 0

# أقل من 0.1 تقريبًا (قد تختلف النتائج) تكون النتائج مجرد ضوضاء.
MAV_OPTICAL_FLOW_confidence_threshold = (0.1)

# التحكم في LED
led = machine.LED("LED_BLUE")
led_state = 0


def update_led():
    global led_state
    led_state = led_state + 1
    if led_state == 10:
        led.on()
    elif led_state >= 20:
        led.off()
        led_state = 0


# إعداد الوصلة
uart = machine.UART(3, UART_BAUDRATE, timeout_char=1000)


# https://github.com/mavlink/c_library_v1/blob/master/checksum.h
def checksum(data, extra):
    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_OPTICAL_FLOW_message_id = 100
MAV_OPTICAL_FLOW_id = 0  # غير مستخدم
MAV_OPTICAL_FLOW_extra_crc = 175


# http://mavlink.org/messages/common#OPTICAL_FLOW
# https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_optical_flow.h
def send_optical_flow_packet(x, y, c):
    global packet_sequence
    temp = struct.pack(
        "<qfffhhbb", 0, 0, 0, 0, int(x), int(y), MAV_OPTICAL_FLOW_id, int(c * 255)
    )
    temp = struct.pack(
        "<bbbbb26s",
        26,
        packet_sequence & 0xFF,
        MAV_system_id,
        MAV_component_id,
        MAV_OPTICAL_FLOW_message_id,
        temp,
    )
    temp = struct.pack("<b31sh", 0xFE, temp, checksum(temp, MAV_OPTICAL_FLOW_extra_crc))
    packet_sequence += 1
    uart.write(temp)
    update_led()


csi0 = csi.CSI()
csi0.reset()  # إعادة ضبط المستشعر وتهيئته.
csi0.pixformat(csi.RGB565)  # تعيين تنسيق البكسل إلى RGB565 (أو GRAYSCALE)
csi0.framesize((64, 64))  # تعيين حجم الإطار إلى 64x64... (أو 64x32)...
csi0.snapshot(time=2000)  # انتظر حتى تدخل الإعدادات حيز التنفيذ.

clock = time.clock()  # أنشئ كائن ساعة (clock) لتتبع معدل الإطارات (FPS).

# إنشاء ذاكرة تخزين مؤقت ثانية للإطار على الكومة (heap).
extra_fb = image.Image(csi0.width(), csi0.height(), csi0.pixformat())
extra_fb.draw_image(csi0.snapshot())

while True:
    clock.tick()  # تتبع عدد الميلي ثانية المنقضية بين استدعاءات snapshot().
    img = csi0.snapshot()  # التقاط صورة وإرجاعها.

    displacement = extra_fb.find_displacement(img)
    extra_fb.draw_image(img)

    # نتائج الإزاحة تكون مشوبة بالضوضاء بدون ترشيح، لذا نتنازل عن بعض الدقة.
    sub_pixel_x = int(-displacement.x_translation * 35)
    sub_pixel_y = int(displacement.y_translation * 53)

    send_optical_flow_packet(sub_pixel_x, sub_pixel_y, displacement.response)

    print(
        "{0:+f}x {1:+f}y {2} {3} FPS".format(
            sub_pixel_x, sub_pixel_y, displacement.response, clock.fps()
        )
    )