Its working now . Seems like it needs rangefinder in order to work the precision land. I have enabled mavlink rangefinder. But I need to decent it at a constant velocity. Is there is any way to disable the rangefinder and do the precision landing?
Hello, after checking the source code of AC_PrecLand, if the PLND_TYPE = 1 (Companion)
void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
{
_distance_to_target = packet.distance;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "handle msg %.3f", _distance_to_target);
if (packet.position_valid == 1) {
if (packet.frame == MAV_FRAME_BODY_FRD) {
if (_distance_to_target > 0) {
_los_meas_body = Vector3f(packet.x, packet.y, packet.z);
_los_meas_body /= _distance_to_target;
} else {
// distance to target must be positive
return;
}
} else {
//we do not support this frame
if (!_wrong_frame_msg_sent) {
_wrong_frame_msg_sent = true;
gcs().send_text(MAV_SEVERITY_INFO,"Plnd: Frame not supported ");
}
return;
}
} else {
// compute unit vector towards target
_los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
_los_meas_body /= _los_meas_body.length();
}
_los_meas_time_ms = timestamp_ms;
_have_los_meas = true;
}
the code show that your frame type should be MAV_FRAME_BODY_FRD distance >0 x y z >0 Position_valid == 1, and
void AC_PrecLand::check_ekf_init_timeout()
{
if (!target_acquired() && _estimator_initialized) {
// we have just got the target in sight
if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) {
// we have lost the target, not enough readings to initialize the EKF
_estimator_initialized = false;
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed");
} else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) {
// the target has been visible for a while, EKF should now have initialized to a good value
_target_acquired = true;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Init Complete");
}
}
}
static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500;
my mavlink_landing_target_t like that
mavlink_landing_target_t req = new mavlink_landing_target_t
{
x = 1,
y = -1,
z = 5,
position_valid = 1,
type = 0,
distance = 30,
frame = 12
};
fast send the packet in LAND mode the your vehicle will move
Iām new to all this, Iām using Speedybeey F405, copter 4.7.0, and by default, it didnāt have the option for Preclanding, I uploaded custom firmware with PrecLanding option, I have set it up, using Raspberry Pi 4B, I the issue Iām facing is I canāt switch to PrcLoiter, and I canāt see an option for ācompanion computerā in the parameter
I donāt have a rangefinder installed, do I need one?
this is my code:
from os import sys, path
sys.path.append(path.dirname(path.dirname(path.abspath(file))))
import time
import math
import argparse
import cv2
import numpy as np
from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal
from pymavlink import mavutil
parser = argparse.ArgumentParser()
parser.add_argument(āāconnectā, default=ā/dev/serial0ā, help=āconnection stringā)
parser.add_argument(āābaudā, default=921600, type=int, help=ābaud rateā)
parser.add_argument(āācameraā, default=1, type=int, help=ācamera index (usually 1 for USB camera)ā)
args = parser.parse_args()
#--------------------------------------------------
#-------------- FUNCTIONS
#--------------------------------------------------
def send_land_message_v2(x_rad=0, y_rad=0, dist_m=0, x_m=0,y_m=0,z_m=0, time_usec=0, target_num=0):
msg = vehicle.message_factory.landing_target_encode(
time_usec, # time target data was processed, as close to sensor capture as possible
target_num, # target num, not used
mavutil.mavlink.MAV_FRAME_BODY_NED, # frame, not used
x_rad, # X-axis angular offset, in radians
y_rad, # Y-axis angular offset, in radians
dist_m, # distance, in meters
0, # Target x-axis size, in radians
0, # Target y-axis size, in radians
x_m, # x float X Position of the landing target on MAV_FRAME
y_m, # y float Y Position of the landing target on MAV_FRAME
z_m, # z float Z Position of the landing target on MAV_FRAME
(1,0,0,0), # q float[4] Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)
2, # type of landing target: 2 = Fiducial marker
1, # position_valid boolean
)
print(msg)
vehicle.send_mavlink(msg)
def send_land_message_v1(x_rad=0, y_rad=0, dist_m=0, time_usec=0, target_num=0):
msg = vehicle.message_factory.landing_target_encode(
int(time_usec), # time target data was processed, as close to sensor capture as possible
target_num, # target num, not used
mavutil.mavlink.MAV_FRAME_BODY_NED, # frame, not used
x_rad, # X-axis angular offset, in radians
y_rad, # Y-axis angular offset, in radians
dist_m, # distance, in meters
0, # Target x-axis size, in radians
0, # Target y-axis size, in radians
)
print(msg)
vehicle.send_mavlink(msg)
def send_distance_message(dist):
msg = vehicle.message_factory.distance_sensor_encode(
0, # time since system boot, not used
1, # min distance cm
10000, # max distance cm
dist, # current distance, must be int
0, # type = laser?
0, # onboard id, not used
mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # must be set to MAV_SENSOR_ROTATION_PITCH_270 for mavlink rangefinder
0 # covariance, not used
)
vehicle.send_mavlink(msg)
def marker_position_to_angle(x, y, z):
angle_x = math.atan2(x,z)
angle_y = math.atan2(y,z)
return (angle_x, angle_y)
def camera_to_uav(x_cam, y_cam):
x_uav = x_cam
y_uav = y_cam
return(x_uav, y_uav)
class ArucoTracker:
def init(self, show_video=False):
self.id_to_find = 2 # Hardcoded ID
self.marker_size = 20 # Marker size in cm
self.show_video = show_video
self.camera_matrix = np.array(
[[945.98202193, 0, 285.21209715],
[0, 952.38344067, 193.88874816],
[0, 0, 1]],
dtype=np.float32
)
self.camera_distortion = np.array(
[0.11206084, 2.50426718, -0.0065591, 0.0029014, -28.9398303],
dtype=np.float32
)
self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
self.parameters = cv2.aruco.DetectorParameters()
self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.parameters)
print(f"Opening camera {0}")
self.cap = cv2.VideoCapture(0)
if not self.cap.isOpened():
print(f"Failed to open camera {0}")
raise RuntimeError(f"Failed to open camera {0}")
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
def track(self, loop=True):
ret, frame = self.cap.read()
if not ret:
print("Failed to grab frame from camera")
return False, 0, 0, 0
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = self.detector.detectMarkers(gray)
if ids is not None and self.id_to_find in ids:
marker_idx = list(ids).index(self.id_to_find)
# Get the corners of the specific marker we want
marker_corners = corners[marker_idx]
# Estimate pose for a single marker
objPoints = np.array([
[-self.marker_size/2, self.marker_size/2, 0],
[self.marker_size/2, self.marker_size/2, 0],
[self.marker_size/2, -self.marker_size/2, 0],
[-self.marker_size/2, -self.marker_size/2, 0]
], dtype=np.float32)
# Get pose for the specific marker
success, rvec, tvec = cv2.solvePnP(
objPoints,
marker_corners[0], # Use the first (and only) marker's corners
self.camera_matrix,
self.camera_distortion,
flags=cv2.SOLVEPNP_IPPE_SQUARE
)
if success:
x = tvec[0][0] # Access the first element of each array
y = tvec[1][0]
z = tvec[2][0]
if self.show_video:
cv2.aruco.drawDetectedMarkers(frame, corners)
cv2.drawFrameAxes(frame, self.camera_matrix, self.camera_distortion,
rvec, tvec, self.marker_size/2)
cv2.imshow('frame', frame)
cv2.waitKey(1)
return True, x * 100, y * 100, z * 100
if self.show_video:
cv2.imshow('frame', frame)
cv2.waitKey(1)
return False, 0, 0, 0
#--------------------------------------------------
#-------------- CONNECTION
#--------------------------------------------------
print(āConnectingā¦ā)
connection_string = args.connect
baud_rate = args.baud
vehicle = connect(connection_string, baud=baud_rate)
#--------------------------------------------------
#-------------- PARAMETERS
#--------------------------------------------------
vehicle.parameters[āPLND_ENABLEDā] = 1
vehicle.parameters[āPLND_TYPEā] = 1 # Mavlink landing backend
freq_send = 15 # Hz
aruco_tracker = ArucoTracker()
time_0 = time.time()
while True:
marker_found, x_cm, y_cm, z_cm = aruco_tracker.track(loop=False)
if True: #marker_found:
x_cm, y_cm = camera_to_uav(x_cm, y_cm)
z_cm = vehicle.location.global_relative_frame.alt*100.0
angle_x, angle_y = marker_position_to_angle(x_cm, y_cm, z_cm)
if time.time() >= time_0 + 1.0/freq_send:
time_0 = time.time()
print("Marker found x = %5.0f cm y = %5.0f cm -> angle_x = %5f angle_y = %5f"%(x_cm, y_cm, angle_x, angle_y))
current_time_usec = int(time.time() * 1e6)
send_land_message_v2(x_rad=angle_x, y_rad=angle_y, dist_m=z_cm*0.01, time_usec=current_time_usec)