当前位置：首页 > article >正文

在Guided模式下给无人机发送命令设置位置速度

article 2024/10/29 0:20:46

命令

在Guided模式下可以控制无人机运动的MAVLink命令为：

SET_POSITION_TARGET_LOCAL_NED
SET_POSITION_TARGET_GLOBAL_INT
SET_ATTITUDE_TARGET (在Guided和Guided_NoGPS模式下支持)
本文仅对SET_POSITION_TARGET_LOCAL_NED做介绍

格式

在官方文档可以查阅到发送该命令的具体格式：
在这里插入图片描述

其中：

time_boot_ms：时间戳（系统启动后的时间），可以用当前时间-系统启动时的时间
coordinate_frame：该字段包含以下值：
有效的选项是：MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9
type_mask：位图，指示车辆应该忽略哪些尺寸。
x：在NED框架中的X位置
y：Y在NED框架中的位置
z：Z在NED帧中的位置（注意，NED中的高度为负）
vx：NED坐标系中的X速度
vy：NED坐标系中的Y速度
vz：NED坐标系中的Z轴速度
afx ：在NED帧中X加速度或力（如果设置type_mask的第10位），单位为米/秒^2或N
afy：Y加速度或力（如果设置type_mask的第10位）在NED帧中以米/秒^2或N为单位
afz ：Z加速度或力（如果设置type_mask的第10位）在NED帧中以米/秒^2或N为单位
yaw：偏航定位点
yaw_rate：偏航率设定值

示例

def set_target_posvel_pc(vel_list,pos_list):
    """
    set target vel and pos w.r.t. the specific frame (only work with Guided Mode)
    """
    master.mav.set_position_target_local_ned_send(
        int(1e3 * (time.time() - boot_time)), # ms since boot
        master.target_system, master.target_component,
        coordinate_frame=mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,
        type_mask=( # Keep vx vy and vz and pos_z and ignore other values
            #1 |
            #2 |
            #4 |
            # DON'T mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
            # DON'T mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
            # DON'T mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
           64 |   # mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
           128 |  # mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
           256 |  # mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
           512 |  # mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET |
           1024 | # mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
           2048   # mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
	), x=pos_list[0], y=pos_list[1], z=pos_list[2], # (x, y WGS84 frame pos - not used), z [m]
        vx=vel_list[0], vy=vel_list[1], vz=vel_list[2], # velocities in body frame [m/s]
        afx=0, afy=0, afz=0, yaw=0, yaw_rate=0

    )

在这段代码中，vel_list是一个包含三个元素的列表，分别代表目标速度在无人机机体坐标系（Body Frame）下的x、y、z轴分量（单位：米/秒）。pos_list是一个包含三个元素的列表，用于指定目标位置，在这个特定的函数调用中，只有z（高度）被使用，x和y被注释说明为WGS84坐标系下的位置。
master是一个与无人机通信的MAVLink连接对象。mav.set_position_target_local_ned_send是一个MAVLink命令，用于发送一个以NED（北-东-地）坐标系为基准的位置和速度目标指令给无人机。NED坐标系是一个本地坐标系，其中N指向北，E指向东，D指向下（与重力方向相反）。
int(1e3 * (time.time() - boot_time))是自无人机启动以来的时间（毫秒）。boot_time是无人机启动的时间戳，time.time()是当前时间戳。这个参数用于确保每条指令都有一个唯一的时间戳。master.target_system 和 master.target_component分别指定目标系统和组件的ID，用于区分不同的无人机或无人机上的不同组件。
coordinate_frame=mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED: 指定坐标系的类型。通过先前的介绍可以知道这里使用的是机体偏移NED坐标系，意味着位置和速度是基于无人机当前位置和姿态的偏移量。
type_mask: 一个位掩码，用于指定哪些字段在指令中是有效的。在这个例子中64 | 128 | 256 | 512 |1024 |2048意味着设置了速度分量（vx, vy, vz）和高度（z）为有效，而忽略了其他所有字段。
x=pos_list[0], y=pos_list[1], z=pos_list[2]: 尽管x和y被提供了，但在这个调用中它们实际上是被忽略的（因为type_mask没有设置它们为有效）。只有z（高度）被用于设置目标高度。
vx=vel_list[0], vy=vel_list[1], vz=vel_list[2]: 设置目标速度分量。
afx=0, afy=0, afz=0, yaw=0, yaw_rate=0: 这些参数分别代表加速度和偏航角及其速率，在这个调用中它们被设置为0，表示不使用这些参数。