Thank you for your response.
I tested VISION_POSITION_ESTIMATE in SITL as you suggested.
However, only the XY position is being rejected, so I am currently testing with a simplified sample code (attached).
At this point, I have not yet been able to identify the reason why it is not being accepted.
I have also made sure that the XY values are not significantly different from the EKF state, but they are still not accepted.
It is possible that I may have a fundamental misunderstanding of the expected behavior or specifications.
If there is anything obviously incorrect in my approach or setup, I would greatly appreciate your guidance.
I have already rebooted the autopilot after changing the EK3_SRC parameters.
# %%
import math
import time
from pymavlink import mavutil
# connection
master = mavutil.mavlink_connection("udp:127.0.0.1:14550")
master.wait_heartbeat()
print("Connected")
# parameter setting
master.param_set_send("EK3_SRC1_POSXY", 6)
master.param_set_send("EK3_SRC1_POSZ", 1)
master.param_set_send("EK3_SRC1_VELXY", 0)
master.param_set_send("EK3_SRC1_VELZ", 0)
master.param_set_send("EK3_SRC1_YAW", 6)
time.sleep(1)
# REQUEST STREAM
master.mav.request_data_stream_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA1,
5,
1,
)
# INIT XY
base_x = 0.0
base_y = 0.0
print("Waiting EKF XY VALID...")
xy_valid = False
start = time.time()
# SEND VISION FOR EKF
while not xy_valid:
timestamp = int(time.time() * 1e6)
t = time.time()
x = base_x + 0.05 * math.sin(t)
y = base_y + 0.05 * math.cos(t)
z = -0.5
# yaw=0
master.mav.vision_position_estimate_send(timestamp, x, y, z, 0.0, 0.0, 0.0)
msg = master.recv_match(type="EKF_STATUS_REPORT", blocking=True, timeout=1.0)
if msg:
flags = msg.flags
if flags & mavutil.mavlink.EKF_POS_HORIZ_REL:
xy_valid = True
print("XY position VALID achieved")
if time.time() - start > 10:
print("EKF failed to initialize XY")
break
# Switch to Guided mode
mode_id = master.mode_mapping()["GUIDED"]
master.set_mode(mode_id)
print("ChangeMODE")
time.sleep(1)
# Arm
master.arducopter_arm()
master.motors_armed_wait()
print("Armed")
# initial position
init = master.recv_match(type="LOCAL_POSITION_NED", blocking=True)
base_x = init.x
base_y = init.y
print(f"Initial EKF: x={base_x}, y={base_y}")
target_x = base_x + 2.0
target_y = base_y
target_z = -0.5
# type_mask
type_mask = (
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
| mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
)
try:
while True:
# Set position
if xy_valid:
master.mav.set_position_target_local_ned_send(
0,
master.target_system,
master.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
type_mask,
target_x,
target_y,
target_z,
0,
0,
0,
0,
0,
0,
0,
0,
)
# Send vision position estimates
timestamp = int(time.time() * 1e6)
# TEST DATA
t = time.time()
x = base_x + 0.05 * math.sin(t)
y = base_y + 0.05 * math.cos(t)
z = -0.5 # NED
roll = 0.0
pitch = 0.0
yaw = math.radians(30)
master.mav.vision_position_estimate_send(timestamp, x, y, z, roll, pitch, yaw)
# LOCAL_POSITION_NED
msg = master.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=0.1)
if msg:
print(f"[EKF] x={msg.x:.2f}, y={msg.y:.2f}, z={msg.z:.2f}")
print(f"[SEND] x={x:.2f}, y={y:.2f}, z={z:.2f}")
# EKF_STATUS_REPORT
msg = master.recv_match(type="EKF_STATUS_REPORT", blocking=True, timeout=1.0)
if msg is not None:
flags = msg.flags
print(f"[EKF_FLAGS] {flags}")
if flags & mavutil.mavlink.EKF_POS_HORIZ_REL:
print(" → XY position VALID")
if flags & mavutil.mavlink.EKF_POS_VERT_ABS:
print(" → Z VALID")
if flags & mavutil.mavlink.EKF_ATTITUDE:
print(" → Attitude OK")
time.sleep(0.1)
except KeyboardInterrupt:
print("Stopping...")
finally:
master.close()
print("Connection closed")
# %%
