Q
@kidswong999
import image, math, pyb, sensor, struct, time
from pyb import UART
# P4 = TXD
sensor.reset() #初始化相机传感器
sensor.set_pixformat(sensor.GRAYSCALE) #设置相机模块的像素模式
sensor.set_framesize(sensor.QQVGA) #设置相机模块的帧大小
sensor.skip_frames(30) #跳过30毫秒的帧
uart = UART(3, 115200,timeout_char = 1000)
#助降系统到飞控消息格式
#1-2同步头 int16 EB90
#3 工作状态
#working_condition
#4-5靶标跟踪误差X int16 像素
tracking_error_X=0
#6-7靶标跟踪误差Y int16 像素
tracking_error_Y=0
#标靶朝向 int16 0.01°
The_target_toward=0
#10-11相机俯仰角控制 int16 0.01°
Camera_pitch_Angle=0
#12-13相机方位角控制 int16 0.01°
Camera_azimuth=0
#14-15标靶北向速度 int16 cm/s
Target_northbound_velocity=0
#16-17标靶东向速度 int16 cm/s
Target_eastbound_velocity=0
#18-19标靶地向速度 int16 cm/s
Target_ground_velocity=0
#20-23标靶北向位置 int32 cm
#dx
#24-27标靶东向位置 int32 cm
#dy
#28-31标靶地向位置 int32 cm
#dz
#32-33标靶航向 uint16 0.01°
The_target_course=0
#34-35无人机航向角速度指令 int16 0.01°/s
Uav_course_angular_velocity_command=0
#36-37无人机前进速度指令 int16 0.01m/s
Forward_speed_command=0
#38-39无人机侧向速度指令 int16 0.01m/s
Lateral_speed_command=0
#40-41无人机垂向速度指令 int16 0.01m/s
Vertical_speed_command=0
#42校验和 uint8
#checksum
Uav_course_Angle=0
#无人机航向角 int16 0.01°
def recive_data():
global Uav_course_Angle
if uart.any():
recive=uart.read()
recive_1=struct.unpack("bbbhhhhhHhhhiiibhhhiiib",recive)
Uav_course_Angle=recive_1[5]
return Uav_course_Angle
def send_data_packet_1():
global temp_1
temp_1 = struct.pack("bbbhhhhhhhhiiihhhhh",
0xEB,
0x90,
working_condition,
tracking_error_X,
tracking_error_Y,
The_target_toward,
Camera_pitch_Angle,
Camera_azimuth,
Target_northbound_velocity,
Target_eastbound_velocity,
Target_ground_velocity,
dx,
dy,
dz,
The_target_course,
Uav_course_angular_velocity_command,
Forward_speed_command,
Lateral_speed_command,
Vertical_speed_command)
def checksum(data):
output=0x00
for i in range(len(data)):
output+=data[i]
cksum_1=output%0x100
return cksum_1
cksum=0x00
def send_data_packet_2():
global temp_2
temp_2 = struct.pack("bbbhhhhhhhhiiihhhhhb",
0xEB,
0x90,
working_condition,
tracking_error_X,
tracking_error_Y,
The_target_toward,
Camera_pitch_Angle,
Camera_azimuth,
Target_northbound_velocity,
Target_eastbound_velocity,
Target_ground_velocity,
dx,
dy,
dz,
The_target_course,
Uav_course_angular_velocity_command,
Forward_speed_command,
Lateral_speed_command,
Vertical_speed_command,
cksum)
uart.write(temp_2)
#def send_data_packet(x,y):
#temp=struct.pack("<bbii", #格式为两个字符两个整形
#0xAA, #帧头1
#0xAE, #帧头2
#int(x), #数据1
#int(y) #数据2
#uart.write(temp) #串口发送
# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）
# c_x 是图像的x中心位置
# c_y 是图像的y中心位置
#kz=实际距离/Tz
#20cm处，kz=200mm/6.85=29.197
#tz=9.89
#实际=kz*tz=9.89*200/6.85=288.759mm
f_x = (2.8 / 3.984) * 160 # 默认值
f_y = (2.8 / 2.952) * 120 # 默认值
c_x = 160 * 0.5 # 默认值(image.w * 0.5)
c_y = 120 * 0.5 # 默认值(image.h * 0.5)
def degrees(radians):
return (180 * radians) / math.pi
clock = time.clock()
while(True):
clock.tick() #开始追踪运行时间
img = sensor.snapshot() #使用相机拍摄一张照片，并返回image对象
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 默认为TAG36H11
img.draw_rectangle(tag.rect(),color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(),color = (0, 255, 0))
print_args=(tag.id())
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(), \
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
# 位置的单位是未知的，旋转的单位是角度
k1=18.867
k2=65.74
if tag.id()==1:
k=k1
else:
k=k2
dx=int((k*tag.x_translation())/10)
dy=int((k*tag.y_translation())/10)
dz=int((k*tag.z_translation())/10)
print_output=(dx, dy, dz, \
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
#print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
#print('dx:%f,dy:%f,dz:%f,rx:%f,ry:%f,rz:%f'%print_output+'\n',tag.id())
working_condition=2
send_data_packet_1()
cksum=checksum(temp_1)
send_data_packet_2()
print(temp_2)
else:
print(0)
dx=0
dy=0
dz=0
working_condition=0
send_data_packet_1()
cksum=checksum(temp_1)
send_data_packet_2()
#k=实际距离/(根号(tx*tx+ty*ty+tz*tz))
#实际tz=18.905172714*7.738752=146.302(150mm)
是希望它能连续的拍到并输出，但是总是一帧能拍到一帧拍不到
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf4\xff\xff\xff\x04\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00R'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf4\xff\xff\xff\x04\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00R'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf4\xff\xff\xff\x04\x00\x00\x00\xe4\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00S'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf4\xff\xff\xff\x04\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00R'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf3\xff\xff\xff\x04\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Q'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf3\xff\xff\xff\x05\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00R'
0
b'\xeb\x90\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf4\xff\xff\xff\x04\x00\x00\x00\xe3\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00R'
0