设定安全区

mavlink id 54 SAFETY_SET_ALLOWED_AREA

发送测试消息python脚本

from pymavlink import mavutil

# 1. 连接无人机（根据实际设备修改连接参数）
# 示例：通过 UDP 连接到无人机
connection = mavutil.mavlink_connection('udpin:0.0.0.0:14550')

# --- 定义立方体的两个对角顶点 (p1 和 p2) ---
# 使用 WGS84 坐标 (经纬度, 海拔)
# 顶点 1 (p1): 经纬度和海拔
p1_lat_deg = 39.998  # 纬度 1
p1_lon_deg = 109.998  # 经度 1
p1_alt_msl_m = 0    # 海拔高度 1 (米)

# 顶点 2 (p2): 经纬度和海拔
p2_lat_deg = 40.002  # 纬度 2
p2_lon_deg = 110.002  # 经度 2
p2_alt_msl_m = 50.0   # 海拔高度 2 (米)

# 2. 转换为MAVLink要求的格式
# 经纬度需要乘以 1e7 并转为整数
p1_lat_int = int(p1_lat_deg * 1e7)
p1_lon_int = int(p1_lon_deg * 1e7)
p1_alt_float = float(p1_alt_msl_m)

p2_lat_int = int(p2_lat_deg * 1e7)
p2_lon_int = int(p2_lon_deg * 1e7)
p2_alt_float = float(p2_alt_msl_m)

# 3. 构建 SAFETY_SET_ALLOWED_AREA 消息
# 根据文档，使用 MAV_FRAME_GLOBAL (0) 来表示 WGS84 坐标系
target_system = 1   
target_component = 1
frame = mavutil.mavlink.MAV_FRAME_GLOBAL # 0: Global coordinate frame (WGS84)

msg = connection.mav.safety_set_allowed_area_encode(
    target_system,
    target_component,
    frame,
    p1_lat_int,  # p1x field (第一个角点的纬度)
    p1_lon_int,  # p1y field (第一个角点的经度)
    p1_alt_float, # p1z field (第一个角点的高度)
    p2_lat_int,  # p2x field (第二个角点的纬度)
    p2_lon_int,  # p2y field (第二个角点的经度)
    p2_alt_float  # p2z field (第二个角点的高度)
)
print("等待无人机心跳...")
connection.wait_heartbeat()
print("无人机已连接，开始发送起飞命令")
# 4. 发送消息
connection.mav.send(msg)

print(f"已发送 SAFETY_SET_ALLOWED_AREA 消息:")
print(f"坐标系: MAV_FRAME_GLOBAL (WGS84)")
print(f"顶点 1 (P1): {p1_lat_deg:.7f}, {p1_lon_deg:.7f}, {p1_alt_msl_m}m MSL")
print(f"顶点 2 (P2): {p2_lat_deg:.7f}, {p2_lon_deg:.7f}, {p2_alt_msl_m}m MSL")