openmv aprltag arducopter landing_target

https://book.openmv.cc/example/18-MAVLink/mavlink-apriltags-landing-target.html

https://mavlink.io/en/messages/common.html#mav_commands

https://mavlink.io/en/messages/common.html#LANDING_TARGET

 

https://ardupilot.org/copter/docs/parameters.html#frame-class

PLND_TYPE

 

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

import image, math, pyb, sensor, struct, time

# 参数#################################################################

uart_baudrate = 115200

MAV_system_id = 1
MAV_component_id = 0x54
MAX_DISTANCE_SENSOR_enable = True

lens_mm = 2.8 # Standard Lens.
lens_to_camera_mm = 22 # Standard Lens.
sensor_w_mm = 3.984 # For OV7725 sensor - see datasheet.
sensor_h_mm = 2.952 # For OV7725 sensor - see datasheet.

# 下面字典中只有标签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
                }

##############################################################################

# Camera Setup

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 z_to_mm(z_translation, tag_size): # z_translation is in decimeters...
    return (((z_translation * 100) * tag_size) / 165) - lens_to_camera_mm

# Link Setup

uart = pyb.UART(3, uart_baudrate, timeout_char = 1000)

# Helper Stuff

packet_sequence = 0

def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master/checksum.h
    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_DISTANCE_SENSOR_message_id = 132
MAV_DISTANCE_SENSOR_min_distance = 1 # in cm
MAV_DISTANCE_SENSOR_max_distance = 10000 # in cm
MAV_DISTANCE_SENSOR_type = 0 # MAV_DISTANCE_SENSOR_LASER
MAV_DISTANCE_SENSOR_id = 0 # unused
MAV_DISTANCE_SENSOR_orientation = 25 # MAV_SENSOR_ROTATION_PITCH_270
MAV_DISTANCE_SENSOR_covariance = 0 # unused
MAV_DISTANCE_SENSOR_extra_crc = 85

# http://mavlink.org/messages/common#DISTANCE_SENSOR
# https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_distance_sensor.h
def send_distance_sensor_packet(tag, tag_size):
    global packet_sequence
    temp = struct.pack("

landing_target_send(self, time_usec, target_num, frame, angle_x, angle_y, distance, size_x, size_y, force_mavlink1=False) method of pymavlink.dialects.v10.ardupilotmega.MAVLink instance
    The location of a landing target. See:
    https://mavlink.io/en/services/landing_target.html
    
    time_usec                 : Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. [us] (type:uint64_t)
    target_num                : The ID of the target if multiple targets are present (type:uint8_t)
    frame                     : Coordinate frame used for following fields. (type:uint8_t, values:MAV_FRAME)
    angle_x                   : X-axis angular offset of the target from the center of the image [rad] (type:float)
    angle_y                   : Y-axis angular offset of the target from the center of the image [rad] (type:float)
    distance                  : Distance to the target from the vehicle [m] (type:float)
    size_x                    : Size of target along x-axis [rad] (type:float)
    size_y                    : Size of target along y-axis [rad] (type:float)
 

 

distance_sensor_send(self, time_boot_ms, min_distance, max_distance, current_distance, type, id, orientation, covariance, force_mavlink1=False) method of pymavlink.dialects.v10.ardupilotmega.MAVLink instance
    Distance sensor information for an onboard rangefinder.
    
    time_boot_ms              : Timestamp (time since system boot). [ms] (type:uint32_t)
    min_distance              : Minimum distance the sensor can measure [cm] (type:uint16_t)
    max_distance              : Maximum distance the sensor can measure [cm] (type:uint16_t)
    current_distance          : Current distance reading [cm] (type:uint16_t)
    type                      : Type of distance sensor. (type:uint8_t, values:MAV_DISTANCE_SENSOR)
    id                        : Onboard ID of the sensor (type:uint8_t)
    orientation               : Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 (type:uint8_t, values:MAV_SENSOR_ORIENTATION)
    covariance                : Measurement variance. Max standard deviation is 6cm. 255 if unknown. [cm^2] (type:uint8_t)