不使用pymavlink , python打包mavlink协议

from struct import pack , unpack
from builtins import object

def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master/checksum.h
    output = 0xFFFF
    for i in range(len(data)):
        tmp = data[i] ^ (output & 0xFF)
        tmp = (tmp ^ (tmp << 4)) & 0xFF
        output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    tmp = extra ^ (output & 0xFF)
    tmp = (tmp ^ (tmp << 4)) & 0xFF
    output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    return output
 
def set_position_target_local_ned_send(time_boot_ms, target_system, target_component, coordinate_frame, type_mask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate, force_mavlink1=False):
    seq = 0xb
    tmp=pack("<6BIfffffffffffHBBB",0xfe,0x35,seq,target_system, target_component,0x54,time_boot_ms,x,y,z,vx,vy,vz,afx, afy, afz, yaw, yaw_rate,type_mask,0, 0, coordinate_frame)
    # print len(tmp)
    tmp = [ord(t) for t in tmp]
  
    crc = checksum(tmp[1:],143)#/python2.7/site-packages/pymavlink/dialects/v10/autoquad.py
    tmp = tmp+[crc&0xff, crc>>8&0xff]
    # ordered_fieldnames = ['time_boot_ms', 'x', 'y', 'z', 'vx', 'vy', 'vz', 'afx', 'afy', 'afz', 'yaw', 'yaw_rate', 'type_mask', 'target_system', 'target_component', 'coordinate_frame']
    # crc_extra = 143
    print hex(crc)
    print ([hex(t) for t in tmp])
set_position_target_local_ned_send(
            0,
            0xff,
            0,     
            8,   # frame
            0xdf8,#0b110111000111,#mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE, # type_mask (only speeds enabled)
            0,0,1, # x, y, z positions (not used)
            0,0,1,#self.vx,self.vy,self.vz, # x, y, z velocity in m/s
            0, 0, 0, # x, y, z acceleration (not used)
            0, 0.9) 

程序运行结果

不使用pymavlink , python打包mavlink协议_第1张图片

 

参考连接

https://blog.csdn.net/CAIYUNFREEDOM/article/details/107077519

https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-attitude-target

https://github.com/mavlink/c_library_v1/blob/master/checksum.h

https://github.com/ArduPilot/pymavlink/tree/697fc96e1a12bbe07c04967647a56ffe66153feb/dialects/v10

 

https://blog.csdn.net/jackyzhousales/article/details/78030847

上述fmt中,支持的格式为:
不使用pymavlink , python打包mavlink协议_第2张图片
注1.q和Q只在机器支持64位操作系统有意义
注2.每个格式前可以有一个数字,表示个数
注3.s格式表示一定长度的字符串,4s表示长度为4的字符串,但是p表示的是pascal字符串
注4.P用来转换一个指针,其长度和机器字长相关
注5.最后一个可以用来表示指针类型的,占4个字节
为了同c中的结构体交换数据,还要考虑有的c或c++编译器使用了字节对齐,通常是以4个字节为单位的32位系统,故而struct根据本地机器字节顺序转换.可以用格式中的第一个字符来改变对齐方式.定义如下:
————————————————
版权声明:本文为CSDN博主「JackyOps」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/jackyzhousales/article/details/78030847

 

 

你可能感兴趣的:(ardupilot,无人驾驶)