DIY Precision Landing with MAVLink and IR

ArduCopter Copter 4.5
1

Hi, i wanted to make my own precison landing method for my arducopter

i know there are 2 methods, MAVlink based (aruco tags) and IR-LOCK, i have problems with both

with the vision based one is that i cant use it in fog, or low visilbity, and with IR-LOCK is that they dont deliver to my country, and i cant find an alternate

i was thinking to use MAVlink based but instad of aruco tags i use IR

if anyone has better idea on how to implement this, or any ideas in genral redarginf this, Please reachout.

2

so im using raspberry pi 4 B (8gb ram) wioth picam v1.3 to detect aruco tags for precision landing, the issue is, the image it receives is so blur, it doesnt even detect the tag, can anyone help me out with this?

note: everything else is working fine, i can see the landing target messages in the mavlink inspector

im providing my code here:

import cv2
import numpy as np
from pymavlink import mavutil
import time
import math
from picamera2 import Picamera2
from libcamera import controls

rate_hz = 60

master = mavutil.mavlink_connection(‘/dev/serial0’, baud=921600,
source_system=1,
source_component=mavutil.mavlink.MAV_COMP_ID_PERIPHERAL)

master.wait_heartbeat()
print(“Heartbeat received from system (system ID %u, component ID %u)” %
(master.target_system, master.target_component))

def initialize_camera():
“”“Initialize the PiCamera2 with lower resolution, performance settings, and fast shutter speed”“”
picam2 = Picamera2()

# Create camera configuration with lower resolution and faster shutter
camera_config = picam2.create_preview_configuration(
    main={
        "size": (320, 240),  # Lower resolution for faster processing
        "format": "BGR888"
    },
    controls={
        "ExposureTime": 1000,  # Faster shutter speed (microseconds)
        "AnalogueGain": 5.0,    # Adjust gain to compensate for faster shutter
        "AeEnable": False,      # Disable automatic exposure
    }
)

picam2.configure(camera_config)
picam2.start()

# 640 x 480
"""
camera_matrix = np.array(
    [[511.21653721,   0.        , 402.65845614],
     [  0.        , 502.42356319, 248.6997854 ],
     [  0.        ,   0.        ,   1.        ]],
    dtype=np.float32
)

dist_coeffs = np.array(
    [-0.07664837, 0.14568316, -0.00554122, 0.02271743, -0.1781878],
    dtype=np.float32
)
"""

# 320 x 240
camera_matrix = np.array(
    [[311.15750696,   0.        ,  163.54023199],
     [  0.        , 311.88688069, 106.81097406 ],
     [  0.        ,   0.        ,   1.        ]],
    dtype=np.float32
)

dist_coeffs = np.array(
    [0.15677319, -0.49959217, -0.01823091, -0.00198025, 1.36993228],
    dtype=np.float32
)


return picam2, camera_matrix, dist_coeffs

def detect_aruco_markers(frame, camera_matrix, dist_coeffs):
“”“Detect ArUco markers and estimate their pose with optimized processing”“”
if frame is None or frame.size == 0:
raise ValueError(“Empty frame received from the camera”)

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
parameters = cv2.aruco.DetectorParameters()

detector = cv2.aruco.ArucoDetector(dictionary, parameters)
corners, ids, rejected = detector.detectMarkers(gray)

rvecs, tvecs = None, None
if ids is not None:
    marker_size = 0.052 
    rvecs, tvecs = [], []
    for i in range(len(corners)):
        success, rvec, tvec = cv2.solvePnP(
            objectPoints=np.array([[-marker_size/2, marker_size/2, 0],
                                 [marker_size/2, marker_size/2, 0],
                                 [marker_size/2, -marker_size/2, 0],
                                 [-marker_size/2, -marker_size/2, 0]]),
            imagePoints=corners[i][0],
            cameraMatrix=camera_matrix,
            distCoeffs=dist_coeffs)
        
        if success:
            rvecs.append(rvec)
            tvecs.append(tvec)

return corners, ids, rvecs, tvecs

def send_landing_target(tvec, camera_matrix):
“”“Send landing target information to the flight controller”“”

distance = float(tvec[2][0])  

cx = camera_matrix[0, 2]
cy = camera_matrix[1, 2]

angle_x = math.atan2(tvec[0][0], tvec[2][0])
angle_y = math.atan2(tvec[1][0], tvec[2][0])

current_time = time.time()
period = 1.0 / rate_hz
if hasattr(send_landing_target, 'last_send_time'):
    if current_time - send_landing_target.last_send_time < period:
        return False

marker_size = 0.04  
size = np.arctan2(marker_size, distance)

master.mav.landing_target_send(
    time_usec=0,
    target_num=1,
    frame=mavutil.mavlink.MAV_FRAME_BODY_NED,
    angle_x=angle_x,
    angle_y=angle_y,
    distance=distance,
    size_x=size,
    size_y=size
)  

send_landing_target.last_send_time = current_time

print("\nSending")
return True

def draw_markers_with_pose(image, corners, ids, rvecs, tvecs, camera_matrix, dist_coeffs):
“”“Draw detected markers and their pose information on the image”“”
if ids is None:
return image

output = image.copy()

cv2.aruco.drawDetectedMarkers(output, corners, ids)

for i in range(len(ids)):
    cv2.drawFrameAxes(output, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.03)
    
    distance = float(tvecs[i][2][0])
    cv2.putText(output, f"ID: {ids[i][0]}, Dist: {distance:.2f}cm", 
                (int(corners[i][0][0][0]), int(corners[i][0][0][1]) - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

return output

if name == ‘main’:
picam2, camera_matrix, dist_coeffs = initialize_camera()

try:
     while True:

        frame = picam2.capture_array()
        corners, ids, rvecs, tvecs = detect_aruco_markers(frame, camera_matrix, dist_coeffs)
        display_frame = frame.copy()
        
        if ids is not None and rvecs is not None and tvecs is not None:

            if len(tvecs) > 0:

                send_landing_target(tvecs[0], camera_matrix)
            
            display_frame = draw_markers_with_pose(
                display_frame, corners, ids, rvecs, tvecs, 
                camera_matrix, dist_coeffs)
        
        cv2.imshow('ArUco Detection', display_frame)
        
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
            
except KeyboardInterrupt:
    print("\nStopping precision landing system...")
finally:
    picam2.stop()
    cv2.destroyAllWindows()