ルーチン説明 18-MAVLink->mavlink_apriltags_landing_target UAV は mavlink を通じて apriltag 固定点着陸を実現します

# 无人机通过mavlink实现apriltag定点降落例程
#
# 这个脚本使用MAVLink协议发送监测到的AprilTag信息到 ArduPilot / PixHawk控制器，使用OpenMV Cam精确着陆。
#
# P4 = TXD

import math
import sensor
import struct
import time
import machine

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

lens_mm = 2.8  # 标配镜头。
lens_to_camera_mm = 22  # 标配镜头。
sensor_w_mm = 4.592  # 用于OV5650感光元件      ov7725 - 3.984  请参见数据手册
sensor_h_mm = 3.423  # 用于OV5650感光元件      ov7725 - 2.952  请参见数据手册

# 下面字典中只有标签ID的标签才会被此代码接受。您可以添加尽可能多的标签ID到下面的字典

# 对于每个标签ID，您需要提供黑色标签边界的长度（毫米）。标签黑色边框正方形的任何一边将工作。

valid_tag_ids = {
    0: 165,  # 8.5" x 11" tag black border size in mm
    1: 165,  # 8.5" x 11" tag black border size in mm
    2: 165,  # 8.5" x 11" tag black border size in mm
}


# 相机的设置
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time=2000)

x_res = 160  # QQVGA
y_res = 120  # QQVGA
f_x = (lens_mm / sensor_w_mm) * x_res
f_y = (lens_mm / sensor_h_mm) * y_res
c_x = x_res / 2
c_y = y_res / 2
h_fov = 2 * math.atan((sensor_w_mm / 2) / lens_mm)
v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm)


def translation_to_mm(translation, tag_size):
    # translation的单位是分米…
    return ((translation * 100) * tag_size) / 210


# 链接设置
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_LANDING_TARGET_message_id = 149
MAV_LANDING_TARGET_min_distance = 1 / 100  # in meters
MAV_LANDING_TARGET_max_distance = 10000 / 100  # in meters
MAV_LANDING_TARGET_frame = 8  # MAV_FRAME_BODY_NED
MAV_LANDING_TARGET_extra_crc = 200


# http://mavlink.org/messages/common#LANDING_TARGET
# https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_landing_target.h
def send_landing_target_packet(tag, dist_mm, w, h):
    global packet_sequence
    temp = struct.pack(
        "<qfffffbb",
        0,
        ((tag.cx() / w) - 0.5) * h_fov,
        ((tag.cy() / h) - 0.5) * v_fov,
        min(
            max(dist_mm * 0.001, MAV_LANDING_TARGET_min_distance),
            MAV_LANDING_TARGET_max_distance,
        ),
        0.0,
        0.0,
        0,
        MAV_LANDING_TARGET_frame,
    )
    temp = struct.pack(
        "<bbbbb30s",
        30,
        packet_sequence & 0xFF,
        MAV_system_id,
        MAV_component_id,
        MAV_LANDING_TARGET_message_id,
        temp,
    )
    temp = struct.pack(
        "<b35sh", 0xFE, temp, checksum(temp, MAV_LANDING_TARGET_extra_crc)
    )
    packet_sequence += 1
    uart.write(temp)


# LED 控制
led_success = machine.LED("LED_GREEN")
led_fail = machine.LED("LED_RED")
led_counter = 0


def update_led(target_found):
    global led_counter

    if target_found:
        led = led_success
        led_fail.off()
    else:
        led = led_fail
        led_success.off()

    if led_counter % 4 == 0:
        led.toggle()

    led_counter += 1


# 主循环

clock = time.clock()
while True:
    clock.tick()
    img = sensor.snapshot()
    tags = sorted(
        img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y),
        key=lambda x: x.w() * x.h(),
        reverse=True,
    )
    target_found = False
    if tags and (tags[0].id() in valid_tag_ids):
        target_found = True
        tag_size = valid_tag_ids[tags[0].id()]
        dist_mm = math.sqrt(
            translation_to_mm(tags[0].x_translation(), tag_size) ** 2
            + translation_to_mm(tags[0].y_translation(), tag_size) ** 2
            + translation_to_mm(tags[0].z_translation(), tag_size) ** 2
        )
        send_landing_target_packet(tags[0], dist_mm, img.width(), img.height())
        img.draw_rectangle(tags[0].rect())
        img.draw_cross(tags[0].cx(), tags[0].cy())
        print("Distance %f mm - FPS %f" % (dist_mm, clock.fps()))
    else:
        print("FPS %f" % clock.fps())

    update_led(target_found)

Singtown Technology OpenMV 公式中国語ドキュメント機能説明: