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)