Hello! 最近在玩树莓派 mpu6050,滤波,发现个很神器的软件 匿名四轴上位机,可以查看波形,但是网上好像都是 C 和 C++ 的下位机程序,就仿照着写了python版的
import serial
import time
from struct import pack,unpack
import smbus
import math
ser = serial.Serial('/dev/ttyAMA0',230400)
#if ser.isOpen == False:
#ser.open()
def send_data(roll,pitch,yaw):
head_buf=["","",""]
head_buf[0]=0X88 #帧头
head_buf[1]=0XA1 #FUN
head_buf[2]=12 #LEN
roll_byte=pack('f',roll) #把float拆成4个字节值,返回列表
pitch_byte=pack('f',pitch) #把float拆成4个字节值
yaw_byte=pack('f',yaw) #把float拆成4个字节值
#print(roll_byte)
#print(roll_byte[3],roll_byte[2],roll_byte[1],roll_byte[0])
send_data=[]
send_data.append(chr(roll_byte[3]).encode('latin1')) #编码,追加到列表 以"latin1"编码可以编大于128的
send_data.append(chr(roll_byte[2]).encode('latin1'))
send_data.append(chr(roll_byte[1]).encode('latin1'))
send_data.append(chr(roll_byte[0]).encode('latin1'))
send_data.append(chr(pitch_byte[3]).encode('latin1')) #编码成b,追加到列表
send_data.append(chr(pitch_byte[2]).encode('latin1'))
send_data.append(chr(pitch_byte[1]).encode('latin1'))
send_data.append(chr(pitch_byte[0]).encode('latin1'))
send_data.append(chr(yaw_byte[3]).encode('latin1')) #编码成b,追加到列表
send_data.append(chr(yaw_byte[2]).encode('latin1'))
send_data.append(chr(yaw_byte[1]).encode('latin1'))
send_data.append(chr(yaw_byte[0]).encode('latin1'))
#----发送head---------
ser.write(chr(head_buf[0]).encode('latin1')) #功能帧用这种
ser.write(chr(head_buf[1]).encode('latin1')) #功能帧用这种
ser.write(chr(head_buf[2]).encode('latin1')) #len 数据的字节数
#ser.write(b'\x88') #功能帧也可以用这种方式发送
#----发送data---------
i=0
while i<12:
ser.write(send_data[i]) #发送数据
i+=1
roll_sum=roll_byte[3]+roll_byte[2]+roll_byte[1]+roll_byte[0]
pitch_sum=pitch_byte[3]+pitch_byte[2]+pitch_byte[1]+pitch_byte[0]
yaw_sum=yaw_byte[3]+yaw_byte[2]+yaw_byte[1]+yaw_byte[0]
sum=(head_buf[0]+head_buf[1]+head_buf[2]+roll_sum+pitch_sum+yaw_sum)
if sum > 256 :
sum = sum % 256 #上位机要求sum uint8 格式
else :
pass
#----发送sum---------
ser.write(chr(sum).encode('latin1')) #发送效验