def send_para(self, msk=(True, True, True, True, True)):
para = self.get_para_from_table()
rtn = 0
if msk[0]:
rtn = self.send_data_by_frame(MsgPrcs.pack_adj_pid_para(para[:4], "X"))
if msk[1]:
rtn = self.send_data_by_frame(MsgPrcs.pack_adj_pid_para(para[4:8], "Y"))
if msk[2]:
rtn = self.send_data_by_frame(MsgPrcs.pack_adj_pid_para(para[8:12], "Z"))
if msk[3]:
rtn = self.send_data_by_frame(MsgPrcs.pack_adj_pid_para(para[12:16], "H"))
if msk[4]:
rtn = self.send_data_by_frame(MsgPrcs.pack_adj_pid_para(para[16:20], "P"))
self.sbar.update(u"参数已经发送(%d)" % rtn)
def get_backdata(self):
'''
从通信模块获取传回的数据
@return: 数据有效，返回值参考L{GroundStation.communication.MessageProcess.unpack_type}，数据无效为(None,None)
'''
buf = self.comm.get_rcvbuf()
if len(buf)>=23:
tmp=buf[-23::]
return MsgPrcs.unpack_type(''.join(buf[-23::]))
else:
return (None, None)
def do_joy_control(self):
rst = self.joy.get_ctrl()
p = rst['pitch']
r = rst['roll']
y = rst['yaw']
t = rst['throttle']
ctrltype = rst['ctrltype']
sd = rst['sd']
self.state_smart_direction = bool(sd)
self.m_checkBox_smart_direction.SetValue(self.state_smart_direction)
self.update_joy_status(rst)
return self.send_data_by_frame(MsgPrcs.pack_control(ctrltype,sd,p,r,y,t))
def process_backdata(self, msgtype, buf):
'''
处理回传数据
@param msgtype: 消息类型
@param buf: 消息数据
'''
for (k,v) in MsgPrcs.PKGTYPE_PID.iteritems():# PID消息
if v == msgtype:
data = MsgPrcs.unpack_5f(buf)
self.update_rcv_pid(k, data[:4])
return
if msgtype == MsgPrcs.PKGTYPE_ATTI:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update_info(data)
elif msgtype == MsgPrcs.PKGTYPE_LOC:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update(None, None, None, None, None, data[0], data[1])
self.update_GE(self.UAVinfo.get())
elif msgtype == MsgPrcs.PKGTYPE_REF:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update_ref(data)
elif msgtype == MsgPrcs.PKGTYPE_CTRL:
data = MsgPrcs.unpack_ctrl(buf)
self.UAVinfo.update_status(data)
elif msgtype == MsgPrcs.PKGTYPE_SPD:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update_spd(data)
elif msgtype == MsgPrcs.PKGTYPE_U0:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update_u0(data)
elif msgtype == MsgPrcs.PKGTYPE_U1:
data = MsgPrcs.unpack_5f(buf)
self.UAVinfo.update_u1(data)
def main_work(self, event):
worklist = self.worklist
a = time.clock()
# 5 Hz Tasks
if (time.clock()-self.timer.last_time) > 1.0/5:
self.timer.last_time=time.clock()
if DISPLAY_XBEE_DATA in worklist:
self.update_rcv_area()
if USING_JOYSTICK in self.worklist:
self.do_joy_control()
# 发送MID=0x09时自动返回，不需重复发送。
if DISPLAY_UAVINFO in self.worklist and USING_JOYSTICK not in self.worklist:
self.send_data_by_frame(MsgPrcs.pack_control(0, self.state_smart_direction))
if DISPLAY_UAVINFO in self.worklist:
self.update_GUI_UAVinfo(self.UAVinfo.get())
# MAIN_TASK_FREQ Hz Tasks
if USING_JOYSTICK in self.worklist:
self.update_joy_status()
if DISPLAY_VIDEO in worklist:
srcimg = self.camcap.get_frame()
wxbmp = util.cvimg_to_wxbmp(srcimg)
wximg = wx.ImageFromBitmap(wxbmp)
memvideo = wx.MemoryDC()
memvideo.SelectObject(wxbmp)
if self.m_menuItem_video_osd.IsChecked():
# draw OSD information on bitmap_video
memvideo.SetTextForeground( wx.BLUE )
memvideo.SetFont( util.WXFONT )
pos = (srcimg.shape[1] - util.PADDING - util.TIME_TEXT_WIDTH, util.PADDING)
memvideo.DrawText(util.get_now(), pos[0], pos[1])
# 设置缩放比例
memvideo.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_video_size[0]),
float(srcimg.shape[0])/float(self.bitmap_video_size[1])
)
self.dc_video.Blit(0, 0, self.bitmap_video_size[0], self.bitmap_video_size[1], memvideo, 0, 0)
memvideo.SelectObject(wx.NullBitmap)
if RECORD_VIDEO in worklist:
self.mov_rec.save_frame(wxbmp)
if DISPLAY_INDEPENDENT_VIDEO in worklist:
self.video_window.update_image_with_info1(wximg, self.UAVinfo.get_information_in_InfoEntries())
# 结束图像传输需要先停止track
if DISPLAY_TRACK_VIDEO in worklist:
memtrack = wx.MemoryDC()
# 显示原始图像
if self.display_track_state == DISPLAY_TRACK_STATE_RAW:
#rstimg = self.get_adjusted_image(srcimg)
rstimg = srcimg
rstbmp = util.cvimg_to_wxbmp(rstimg)
# 正在框选状态
elif self.display_track_state == DISPLAY_TRACK_STATE_SELECTION:
assert self.frozen_frame is not None, 'Frozen frame is none.'
rectimg = self.get_dragging_image(self.frozen_frame,self.drag_info.get_drag_data())
rstbmp = util.cvimg_to_wxbmp(rectimg)
# 显示目标追踪结果
elif self.display_track_state == DISPLAY_TRACK_STATE_RESULT:
track_mode = self.m_choice_track_mode.GetStringSelection()
display_process = self.m_menuItem_track_display_process.IsChecked()
if track_mode == 'template':
method = METHOD.TEMPLATEMATCH
elif track_mode == 'meanshift':
method = METHOD.MEANSHIFT
elif track_mode == 'gray-meanshift':
method = METHOD.GRAYMEANSHIFT
else:
method = METHOD.OPTICALFLOW
matchimg, center, res = self.objmatch.process(method, srcimg)
if display_process:
rstbmp = util.cvimg_to_wxbmp(res)
else:
rstbmp = util.cvimg_to_wxbmp(matchimg)
# # 模板匹配模式
# if track_mode == 'template':
# matchimg, center, res = self.objmatch.do_tpl_match(srcimg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(res)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# # 边缘检测-模板匹配模式
# elif track_mode == 'edge-tpl':
# matchimg, center, edgeimg = self.objmatch.do_edge_match(srcimg, arg=self.edge_arg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(edgeimg)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# # MeanShift匹配模式
# elif track_mode == 'meanshift':
# matchimg, center, prj_img = self.objmatch.do_meanshift(srcimg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(prj_img)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# # 多目标MeanShift匹配模式
# elif track_mode == 'multi-meanshift':
# matchimg, center, prj_img = self.objmatch.do_multi_meanshift(srcimg, arg=self.multimean_arg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(prj_img)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# elif track_mode == 'gray-meanshift':
# matchimg, center, prj_img = self.objmatch.do_gray_meanshift(srcimg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(prj_img)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# elif track_mode == 'optical-flow':
# matchimg, center, prj_img = self.objmatch.do_optical_flow(srcimg)
# if display_process:
# rstbmp = util.cvimg_to_wxbmp(prj_img)
# else:
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# # 混合匹配模式
# elif track_mode == 'mix':
# matchimg, center, _ = self.objmatch.do_mix(srcimg, multimean_arg=self.multimean_arg, edgetpl_arg=self.edge_arg)
# rstbmp = util.cvimg_to_wxbmp(matchimg)
# 更新track bitmap 界面
memtrack.SelectObject(rstbmp)
memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]),
float(srcimg.shape[0])/float(self.bitmap_track_size[1]))
self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0)
a = time.clock()
if TRACK_OBJECT in worklist:
self.trackctrl.add_pt(center)
self.trackctrl.update_h(self.UAVinfo.get()['height'])
self.trackctrl.get_u()
rstimg = self.objmatch.draw_circles(matchimg, self.trackctrl.pts[-1], color='GREEN', radius=10)
rstbmp = util.cvimg_to_wxbmp(rstimg)
memtrack.SelectObject(rstbmp)
memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]),
float(srcimg.shape[0])/float(self.bitmap_track_size[1]))
self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0)
n = time.clock()
# print('[work time]%4.4f [cir time]%4.4f'%((n-a)*1000,(n-self.lasttime)*1000))
self.lasttime = n
def send_ref(self,u):
x,y,z = u
h=0
rtn = self.send_data_by_frame(MsgPrcs.pack_ref(x,y,z,h))
self.sbar.update(u'给定<r=%.4f,p=%.4f,y=%.4f>已发送(%d)'%(x,y,z,rtn))
def main_work(self, event):
worklist = self.worklist
a = time.clock()
# MAIN_TASK_FREQ Hz Tasks
if USING_JOYSTICK in self.worklist:
self.update_joy_status()
if DISPLAY_VIDEO in worklist:
srcimg = self.camcap.get_frame()
if len(self.cambuf)==0:
self.cambuf.append(srcimg)
self.cambuf.append(srcimg)
# self.cambuf.append(srcimg)
# print('cambuf length:%d'%len(self.cambuf))
wxbmp = util.cvimg_to_wxbmp(srcimg)
wximg = wx.ImageFromBitmap(wxbmp)
memvideo = wx.MemoryDC()
memvideo.SelectObject(wxbmp)
if self.m_menuItem_video_osd.IsChecked():
# draw OSD information on bitmap_video
memvideo.SetTextForeground( wx.BLUE )
memvideo.SetFont( util.WXFONT )
pos = (srcimg.shape[1] - util.PADDING - util.TIME_TEXT_WIDTH, util.PADDING)
memvideo.DrawText(util.get_now(), pos[0], pos[1])
# 设置缩放比例
memvideo.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_video_size[0]),
float(srcimg.shape[0])/float(self.bitmap_video_size[1])
)
self.dc_video.Blit(0, 0, self.bitmap_video_size[0], self.bitmap_video_size[1], memvideo, 0, 0)
memvideo.SelectObject(wx.NullBitmap)
if RECORD_VIDEO in worklist:
self.mov_rec.save_frame(wxbmp)
if DISPLAY_INDEPENDENT_VIDEO in worklist:
self.video_window.update_image(wximg, self.UAVinfo.get_information_in_InfoEntries())
# 结束图像传输需要先停止track
if DISPLAY_TRACK_VIDEO in worklist:
memtrack = wx.MemoryDC()
#图像滤波
self.cambuf.append(srcimg)
self.cambuf.pop(0)
a=time.clock()
srcimg = img_filter(self.cambuf)
# print '%.6f'%((time.clock()-a)*1000)
# self.cambuf.append(srcimg)
# self.cambuf.pop(0)
# 显示原始图像
if self.display_track_state == DISPLAY_TRACK_STATE_RAW:
#rstimg = self.get_adjusted_image(srcimg)
rstimg = srcimg
rstbmp = util.cvimg_to_wxbmp(rstimg)
# 正在框选状态
elif self.display_track_state == DISPLAY_TRACK_STATE_SELECTION:
assert self.frozen_frame is not None, 'Frozen frame is none.'
rectimg = self.get_dragging_image(self.frozen_frame,self.drag_info.get_drag_data())
rstbmp = util.cvimg_to_wxbmp(rectimg)
# 显示目标追踪结果
elif self.display_track_state == DISPLAY_TRACK_STATE_RESULT:
track_mode = self.m_choice_track_mode.GetStringSelection()
display_process = self.m_menuItem_track_display_process.IsChecked()
if track_mode == 'template':
method = METHOD.TEMPLATEMATCH
elif track_mode == 'meanshift':
method = METHOD.MEANSHIFT
elif track_mode == 'gray-meanshift':
method = METHOD.GRAYMEANSHIFT
else:
method = METHOD.OPTICALFLOW
matchimg, center, res = self.objmatch.process(method, srcimg)
if display_process:
rstbmp = util.cvimg_to_wxbmp(res)
tmpimg = res
else:
rstbmp = util.cvimg_to_wxbmp(matchimg)
tmpimg = matchimg
if TRACK_OBJECT in worklist:
self.trackctrl.add_pt(center)
# TODO:MeanShift-OpticalFlow 卡尔曼
# 更新track bitmap 界面
memtrack.SelectObject(rstbmp)
memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]),
float(srcimg.shape[0])/float(self.bitmap_track_size[1]))
self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0)
a = time.clock()
# 5 Hz Tasks
if (time.clock()-self.timer.last_time) > (1.0/TASK_LOW_FREQ):
self.timer.last_time=time.clock()
if DISPLAY_XBEE_DATA in worklist:
self.update_rcv_area()
if USING_JOYSTICK in self.worklist:
self.do_joy_control()
# 发送MID=0x09时自动返回，不需重复发送。
elif DISPLAY_UAVINFO in self.worklist:
self.send_data_by_frame(MsgPrcs.pack_control(0, self.state_smart_direction))
if DISPLAY_UAVINFO in self.worklist:
self.update_GUI_UAVinfo(self.UAVinfo.get(-2))
if TRACK_OBJECT in worklist:
now_height = self.UAVinfo.get().height
self.trackctrl.update_h(3 if math.isnan(now_height) else now_height)
nt = self.UAVinfo.get(-2).uavtime
du = self.trackctrl.get_u(nt, self.camera_pt_pitch)
rstimg = self.objmatch.draw_circles(tmpimg, self.trackctrl.pts[-1], color='GREEN', radius=10)
rstbmp = util.cvimg_to_wxbmp(rstimg)
memtrack.SelectObject(rstbmp)
memtrack.SetUserScale(float(srcimg.shape[1])/float(self.bitmap_track_size[0]),
float(srcimg.shape[0])/float(self.bitmap_track_size[1]))
self.dc_track.Blit(0, 0, self.bitmap_track_size[0], self.bitmap_track_size[1], memtrack, 0, 0)
self.send_ref(du)
n = time.clock()
# print('[work time]%4.4f [cir time]%4.4f'%((n-a)*1000,(n-self.lasttime)*1000))
self.lasttime = n
def send_pt_reference(self):
pitch = float(self.m_spinCtrl_PT_pitch.GetValue())
roll = float(self.m_spinCtrl_PT_roll.GetValue())
self.camera_pt_pitch = pitch
return self.send_data_by_frame(MsgPrcs.pack_pt(p=pitch, r=roll))
def get_pid(self):
data = MsgPrcs.pack_get_pid()
rtn = self.send_data_by_frame(data)
self.sbar.update(u"参数已经固定(%d)" % rtn)
def set_down_para(self):
data = MsgPrcs.pack_set_pid()
rtn = self.send_data_by_frame(data)
self.sbar.update(u"参数已经固定(%d)" % rtn)
