<filename>worker/worker_wifi.py
# -*- coding: utf-8 -*-
'''
Copyright 2020, Ainstein Inc. All Rights Reserved


This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.

Contact: <EMAIL>
 '''
import socketserver
import smokesignal
from queue import Queue
from xmodem import XMODEM
import os
import math
import logging
from worker.msg.msg_485 import *
from worker.worker_base import *
from worker.msg.msg_detail import *
import socket
import datetime
import threading


class WifiTCPHandler(socketserver.BaseRequestHandler):
    def __init__(self, request, client_address, server):
        self.is_run = True
        self.cfg_cmds = None
        self.firm_path = None
        self.sbl_path = None
        self.query_desc = None
        self.timeout = 0.01
        self.ip_port = ""
        self.radar_timeout = WorkerWifi.radar_timeout
        self.comm_timeout = WorkerWifi.comm_timeout
        # 心跳检测间隔
        self.ds = DeviceState()
        #
        self.alive_time = 0
        #
        self.is_radar = False
        super(WifiTCPHandler, self).__init__(request, client_address, server)

    def send(self, data):
        # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ")
        # print("send:", end="")
        # for b in data:
        #     print("%02x " % b, end="")
        # print("")
        self.request.sendall(data)

    def recv(self, count):
        data = None
        try:
            data = self.request.recv(count)
        except socket.timeout:
            pass
        if data is not None and len(data) > 0:
            # 收到数据即认为雷达存活
            self.alive_time = time.time()
            # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ")
            # print("recv:", end="")
            # for b in data:
            #     print("%02x " % b, end="")
            # print("")
        return data

    def end(self):
        self.is_run = False

    def handle(self):
        print("Radar connected via WiFi", self.client_address)
        # 将新的TCP连接记录到字典
        # self.ip_port = self.client_address[0] + ":" + str(self.client_address[1])
        self.ip_port = self.client_address[0]
        if self.ip_port in WorkerWifi.dict_clients.keys():
            WorkerWifi.dict_clients[self.ip_port].end()
            time.sleep(2)
        WorkerWifi.dict_clients[self.ip_port] = self
        # 设置超时时间
        self.request.settimeout(self.timeout)
        #
        self.alive_time = time.time()
        # 主循环
        while self.is_run:
            if time.time() - self.alive_time >= 40:
                print("not alive")
                break
            time.sleep(0.005)
            # 心跳检测（获取版本号）
            if self.ds.need_get_version():
                self.send(bytes("getVersion\n", "utf-8"))
                ret, msg_detail = self.get_msg485(CMD_485_VERSION, timeout=self.radar_timeout)
                if ret:
                    WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                    self.ds.set_radar()
            # 下发CFG
            try:
                if self.cfg_cmds is not None:
                    self.send_cfg_cmds()
                    self.cfg_cmds = None
                    self.ds.reset()
                    time.sleep(5)
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, str(e)))
                break
            # update firmware
            try:
                if self.firm_path is not None:
                    self.update_firmware()
                    self.firm_path = None
                    self.ds.reset()
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_FIRM, str(e)))
                break
            # update sbl
            try:
                if self.sbl_path is not None:
                    self.update_sbl()
                    self.sbl_path = None
                    self.ds.reset()
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_SBL, str(e)))
                break
            # 接收目标数据
            if self.ds.is_radar() and self.ds.need_get_target():
                try:
                    if WorkerWifi.debug_target:
                        self.send(bytes("getDebugTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_DEBUG_TARGET, timeout=self.comm_timeout)
                    elif WorkerWifi.detail_target:
                        self.send(bytes("getDetailTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_DETAIL_TARGET, timeout=self.comm_timeout)
                    else:
                        self.send(bytes("getTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_TARGET, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                except Exception as e:
                    print(e)
                    break
            # 查询
            if self.query_desc is not None:
                if self.ds.is_radar():
                    self.send(bytes("getConf\n", "utf-8"))
                    ret, msg_detail = self.get_msg485(CMD_485_CONFIG, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                    self.send(bytes("getWorkParam\n", "utf-8"))
                    ret, msg_detail = self.get_msg485(CMD_485_PARAM, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                self.query_desc = None
        # 从字典删除该TCP连接
        del WorkerWifi.dict_clients[self.ip_port]
        print("Radar Disconnected:", self.ip_port)
        WorkerWifi.queue_radar_disconnect.put(self.ip_port)

    def get_msg485(self, cmd_code, timeout=0.2):
        # 收消息，有超时时间
        time_now = time.time()
        cache_485 = bytes()
        while time.time() - time_now < timeout:
            data = self.recv(128)
            if data is not None and len(data) > 0:
                cache_485 += data
                cache_485, msgs_485 = Msg485Recv.parse_data(cache_485)
                for msg_485 in msgs_485:
                    if msg_485.is_msg(cmd_code):
                        msg_detail = MsgDetail.parse_data(cmd_code, msg_485.frame_data)
                        return True, msg_detail
            else:
                time.sleep(0.005)
        return False, None

    def send_cfg_cmds(self):
        ret_desc = ""
        cur_cmd_idx = 1
        for cmd in self.cfg_cmds:
            # 发送命令
            self.send(bytes(cmd + "\n", "utf-8"))
            if cmd == "closeWifi":
                # 对关闭wifi做特殊超时处理
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=5)
            else:
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=self.comm_timeout)
            if not ret:
                ret_desc += cmd + " : no data received\r\n"
            elif msg_detail.response != 0x01:
                ret_desc += cmd + " : %02X" % msg_detail.response + "\r\n"
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG,
                                                int(cur_cmd_idx * 100 / len(self.cfg_cmds))))
            cur_cmd_idx += 1
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))

    def exec_filter(self): #clutter filter executor 
        ret_desc = ""
        # 1. 查目标
        targets = []
        self.send(bytes("getTarget\n", "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_TARGET, timeout=self.comm_timeout)
        if ret:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG, 20))
            targets = msg_detail.tags[0].targets
        else:
            ret_desc = "目标获取失败"
            WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))
            return
        # 2. 查配置
        cfgs = ""
        self.send(bytes("getWorkParam\n", "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_PARAM, timeout=self.comm_timeout)
        if ret:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG, 40))
            cfgs = msg_detail.cmds
        else:
            ret_desc = "获取CFG参数失败"
            WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))
            return
        # 3. 改配置
        cmds = cfgs.split("\n")
        new_cmds = []
        for cmd in cmds:
            if 'SceneryParam' in cmd:
                segs = cmd.split(" ")
                if len(segs) == 7:
                    # 老语法，统一到新语法，添加0
                    segs.append("0")
                if segs[-1] == "0":
                    new_cmds.append(" ".join(segs))
                    # 添加杂波点 clutter filter
                    for t in targets:
                        new_cmds.append("SceneryParam %f %f %f %f %f %f 1" %
                                        (t.x + self.filter_region[0], t.x + self.filter_region[1],
                                         t.y + self.filter_region[2], t.y + self.filter_region[3],
                                         t.z + self.filter_region[4], t.z + self.filter_region[5]))
            else:
                new_cmds.append(cmd)
        #
        cur_cmd_idx = 1
        for cmd in new_cmds:
            # 发送命令
            self.send(bytes(cmd + "\n", "utf-8"))
            if cmd == "closeWifi":
                # 对关闭wifi做特殊超时处理
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=5)
            else:
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=self.comm_timeout)
            if not ret:
                ret_desc += cmd + " : no data received\r\n"
            elif msg_detail.response != 0x01:
                ret_desc += cmd + " : %02X" % msg_detail.response + "\r\n"
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG,
                                                40 + int(cur_cmd_idx * 60 / len(new_cmds))))
            cur_cmd_idx += 1
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))

    def update_firmware(self):
        ret_desc = ""
        # 1. 发送ReadyUpdate
        ret = False
        for i in range(5):
            self.send(bytes("ReadyUpdate\n", "utf-8"))
            ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=1)
            if ret and msg_detail.response == 0x21 and msg_detail.crc == 0xcb:
                ret = True
                break
        if not ret:
            ret_desc = "ReadyUpdate命令未收到响应"
        else:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 10))
            # 2. 发送Update
            self.send(bytes("Update\n", "utf-8"))
            ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
            if not ret or msg_detail.response != 0x22 or msg_detail.crc != 0xcc:
                ret_desc = "Update命令未收到响应"
            else:
                WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 20))
                # 3. XModem协议传输文件
                file = open(self.firm_path, "rb")
                file_size = os.path.getsize(self.firm_path)
                packet_num = math.ceil(file_size / WorkerWifi.packet_size)

                def getc(size, timeout=5):
                    cache = bytes()
                    x_time = time.time()
                    while time.time() - x_time < timeout:
                        data = self.recv(size)
                        if data is not None and len(data) > 0:
                            cache += data
                            if len(cache) >= size:
                                return cache[0: size]
                        else:
                            time.sleep(0.005)
                    return None

                def putc(data, timeout=1):
                    return self.send(data)

                def callback_func(total_packets, success_count, error_count):
                    WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM,
                                                        20 + int(success_count * 70 / packet_num)))

                if WorkerWifi.packet_size == 128:
                    xmodem = XMODEM(getc, putc, mode='xmodem')
                else:
                    xmodem = XMODEM(getc, putc, mode='xmodem1k')
                xmodem_ret = xmodem.send(file, timeout=5, callback=callback_func)
                file.close()
                if not xmodem_ret:
                    ret_desc = "XModem传输错误"
                else:
                    # 4. 确认响应
                    ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
                    if not ret or msg_detail.response != 0x23 or msg_detail.crc != 0xcd:
                        ret_desc = "未收到固件更新完成响应"
                    else:
                        WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 100))
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_FIRM, ret_desc))

    def update_sbl(self):
        ret_desc = ""
        # 1. 发送sbl升级请求
        file_size = os.path.getsize(self.sbl_path)
        packet_num = math.ceil(file_size / WorkerWifi.packet_size)
        self.send(bytes("updateSbl %d\n" % file_size, "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
        if not ret:
            ret_desc = "updateSbl命令未收到响应"
        elif msg_detail.response != 0x01:
            ret_desc = "updateSbl命令响应错误 : %02X" % msg_detail.response
        else:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_SBL, 10))
            # 2. 传输文件
            file = open(self.sbl_path, "rb")
            for i in range(packet_num):
                if i < packet_num - 1:
                    cmd_bytes = file.read(WorkerWifi.packet_size)
                else:
                    cmd_bytes = file.read()
                for j in range(5):
                    self.send(Msg485Send(1, frame_idx=i+1).get_cmd(CMD_485_SBL, cmd_bytes))
                    ret, msg_detail = self.get_msg485(CMD_485_SBL, timeout=5)
                    if not ret:
                        ret_desc = "sbl升级包未收到响应"
                    elif msg_detail.response != 0x01:
                        ret_desc = "sbl升级包响应错误 : %02X" % msg_detail.response
                    else:
                        ret_desc = ""
                        WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_SBL, 10 + int((i + 1) * 90 / packet_num)))
                        break
                if ret_desc != "":
                    break
            file.close()
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_SBL, ret_desc))


class WifiTCPServer(Thread):
    def __init__(self, server):
        """
        构造函数
        """
        super(WifiTCPServer, self).__init__()
        self.server = server

    def end(self):
        """
        停止接收数据
        """
        self.server.shutdown()
        self.server.server_close()
        return True

    def run(self):
        self.server.serve_forever()


class WorkerWifi(WorkerBase):
    """
    网络数据接收器
    """
    # TCP连接字典
    dict_clients = {}
    # 线程共享数据
    queue_msg = Queue()
    queue_progress_rate = Queue()
    queue_progress_rets = Queue()
    queue_radar_disconnect = Queue()
    #
    radar_timeout = 1
    comm_timeout = 0.2
    packet_size = 128
    debug_target = False
    detail_target = False

    def __init__(self, ip, port, radar_timeout=1, comm_timeout=0.2, debug_target=False, detail_target=False, packet_size=128):
        """
        构造函数
        """
        super(WorkerWifi, self).__init__()
        self.ip = ip
        self.port = port
        self.tcp_server = None
        WorkerWifi.debug_target = debug_target
        WorkerWifi.detail_target = detail_target
        WorkerWifi.radar_timeout = radar_timeout
        WorkerWifi.comm_timeout = comm_timeout
        WorkerWifi.packet_size = packet_size

    def query(self, query_desc):
        self.query_desc = query_desc

    def cfg_config(self, cfg_cmds, cfg_filter):
        """
        cfg配置
        :param cfg_cmds:
        :param cfg_filter:
        :return:
        """
        self.cfg_cmds = cfg_cmds.split("\n")
        self.cfg_filter = cfg_filter

    def firm_update(self, firm_path, firm_filter):
        """
        固件更新
        :param firm_path:
        :param firm_filter:
        :return:
        """
        self.firm_path = firm_path
        self.firm_filter = firm_filter

    def sbl_update(self, sbl_path, sbl_filter):
        self.sbl_path = sbl_path
        self.sbl_filter = sbl_filter

    def init(self):
        """
        测试网络是否能够正常连接
        :return:
        """
        try:
            socketserver.TCPServer.allow_reuse_address = True
            server = socketserver.ThreadingTCPServer((self.ip, self.port), WifiTCPHandler)
            self.tcp_server = WifiTCPServer(server)
        except Exception as e:
            print(e)
            return False
        return True

    def kick_client(self, desc):
        if desc in WorkerWifi.dict_clients.keys():
            WorkerWifi.dict_clients[desc].end()

    def kick_all_client(self):
        for desc, client in WorkerWifi.dict_clients.items():
            client.end()

    def reset_descs(self, ids_485):
        return

    def add_485id(self, id_485):
        return

    def del_485id(self, id_485):
        self.kick_client(id_485)

    def run(self):
        self.tcp_server.start()
        while self.is_run:
            try:
                time.sleep(0.005)
                # 消息通知
                while not WorkerWifi.queue_msg.empty():
                    p = WorkerWifi.queue_msg.get()
                    smokesignal.emit('msg_signal', p[0], p[1])
                # 发送cfg配置
                if self.cfg_cmds is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.cfg_filter:
                            tcp_client.cfg_cmds = self.cfg_cmds
                            descs.append(desc)
                    self.cfg_cmds = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_CFG, ";".join(descs)))
                # 杂波滤除 CLUTTER FILTER
                if self.filter_region is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.filter_filter:
                            tcp_client.filter_region = self.filter_region
                            descs.append(desc)
                    self.filter_region = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_CFG, ";".join(descs)))

                # update firmware
                if self.firm_path is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.firm_filter:
                            tcp_client.firm_path = self.firm_path
                            descs.append(desc)
                    self.firm_path = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_FIRM, ";".join(descs)))
                # update firmware
                if self.sbl_path is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.sbl_filter:
                            tcp_client.sbl_path = self.sbl_path
                            descs.append(desc)
                    self.sbl_path = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_SBL, ";".join(descs)))
                #
                if self.query_desc is not None:
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc == self.query_desc:
                            tcp_client.query_desc = self.query_desc
                    self.query_desc = None
                #
                while not WorkerWifi.queue_progress_rate.empty():
                    p = WorkerWifi.queue_progress_rate.get()
                    smokesignal.emit('progress_rate_signal',p[0], p[1], p[2])
                while not WorkerWifi.queue_progress_rets.empty():
                    p = WorkerWifi.queue_progress_rets.get()
                    smokesignal.emit('progress_result_signal',p[0], p[1], p[2])
                # 客户端退出通知
                while not WorkerWifi.queue_radar_disconnect.empty():
                    p = WorkerWifi.queue_radar_disconnect.get()
                    smokesignal.emit('client_exit_signal',p)
            except Exception as e:
                print(e)
        self.kick_all_client()
        self.tcp_server.end()
        #
        WorkerWifi.dict_clients.clear()
        WorkerWifi.queue_msg.queue.clear()
        WorkerWifi.queue_progress_rate.queue.clear()
        WorkerWifi.queue_progress_rets.queue.clear()
        WorkerWifi.queue_radar_disconnect.queue.clear()

<filename>worker/worker_wifi.py
# -*- coding: utf-8 -*-
'''
Copyright 2020, Ainstein Inc. All Rights Reserved


This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.

Contact: <EMAIL>
 '''
import socketserver
import smokesignal
from queue import Queue
from xmodem import XMODEM
import os
import math
import logging
from worker.msg.msg_485 import *
from worker.worker_base import *
from worker.msg.msg_detail import *
import socket
import datetime
import threading


class WifiTCPHandler(socketserver.BaseRequestHandler):
    def __init__(self, request, client_address, server):
        self.is_run = True
        self.cfg_cmds = None
        self.firm_path = None
        self.sbl_path = None
        self.query_desc = None
        self.timeout = 0.01
        self.ip_port = ""
        self.radar_timeout = WorkerWifi.radar_timeout
        self.comm_timeout = WorkerWifi.comm_timeout
        # 心跳检测间隔
        self.ds = DeviceState()
        #
        self.alive_time = 0
        #
        self.is_radar = False
        super(WifiTCPHandler, self).__init__(request, client_address, server)

    def send(self, data):
        # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ")
        # print("send:", end="")
        # for b in data:
        #     print("%02x " % b, end="")
        # print("")
        self.request.sendall(data)

    def recv(self, count):
        data = None
        try:
            data = self.request.recv(count)
        except socket.timeout:
            pass
        if data is not None and len(data) > 0:
            # 收到数据即认为雷达存活
            self.alive_time = time.time()
            # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ")
            # print("recv:", end="")
            # for b in data:
            #     print("%02x " % b, end="")
            # print("")
        return data

    def end(self):
        self.is_run = False

    def handle(self):
        print("Radar connected via WiFi", self.client_address)
        # 将新的TCP连接记录到字典
        # self.ip_port = self.client_address[0] + ":" + str(self.client_address[1])
        self.ip_port = self.client_address[0]
        if self.ip_port in WorkerWifi.dict_clients.keys():
            WorkerWifi.dict_clients[self.ip_port].end()
            time.sleep(2)
        WorkerWifi.dict_clients[self.ip_port] = self
        # 设置超时时间
        self.request.settimeout(self.timeout)
        #
        self.alive_time = time.time()
        # 主循环
        while self.is_run:
            if time.time() - self.alive_time >= 40:
                print("not alive")
                break
            time.sleep(0.005)
            # 心跳检测（获取版本号）
            if self.ds.need_get_version():
                self.send(bytes("getVersion\n", "utf-8"))
                ret, msg_detail = self.get_msg485(CMD_485_VERSION, timeout=self.radar_timeout)
                if ret:
                    WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                    self.ds.set_radar()
            # 下发CFG
            try:
                if self.cfg_cmds is not None:
                    self.send_cfg_cmds()
                    self.cfg_cmds = None
                    self.ds.reset()
                    time.sleep(5)
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, str(e)))
                break
            # update firmware
            try:
                if self.firm_path is not None:
                    self.update_firmware()
                    self.firm_path = None
                    self.ds.reset()
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_FIRM, str(e)))
                break
            # update sbl
            try:
                if self.sbl_path is not None:
                    self.update_sbl()
                    self.sbl_path = None
                    self.ds.reset()
            except Exception as e:
                WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_SBL, str(e)))
                break
            # 接收目标数据
            if self.ds.is_radar() and self.ds.need_get_target():
                try:
                    if WorkerWifi.debug_target:
                        self.send(bytes("getDebugTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_DEBUG_TARGET, timeout=self.comm_timeout)
                    elif WorkerWifi.detail_target:
                        self.send(bytes("getDetailTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_DETAIL_TARGET, timeout=self.comm_timeout)
                    else:
                        self.send(bytes("getTarget\n", "utf-8"))
                        ret, msg_detail = self.get_msg485(CMD_485_TARGET, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                except Exception as e:
                    print(e)
                    break
            # 查询
            if self.query_desc is not None:
                if self.ds.is_radar():
                    self.send(bytes("getConf\n", "utf-8"))
                    ret, msg_detail = self.get_msg485(CMD_485_CONFIG, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                    self.send(bytes("getWorkParam\n", "utf-8"))
                    ret, msg_detail = self.get_msg485(CMD_485_PARAM, timeout=self.comm_timeout)
                    if ret:
                        WorkerWifi.queue_msg.put((self.ip_port, msg_detail))
                self.query_desc = None
        # 从字典删除该TCP连接
        del WorkerWifi.dict_clients[self.ip_port]
        print("Radar Disconnected:", self.ip_port)
        WorkerWifi.queue_radar_disconnect.put(self.ip_port)

    def get_msg485(self, cmd_code, timeout=0.2):
        # 收消息，有超时时间
        time_now = time.time()
        cache_485 = bytes()
        while time.time() - time_now < timeout:
            data = self.recv(128)
            if data is not None and len(data) > 0:
                cache_485 += data
                cache_485, msgs_485 = Msg485Recv.parse_data(cache_485)
                for msg_485 in msgs_485:
                    if msg_485.is_msg(cmd_code):
                        msg_detail = MsgDetail.parse_data(cmd_code, msg_485.frame_data)
                        return True, msg_detail
            else:
                time.sleep(0.005)
        return False, None

    def send_cfg_cmds(self):
        ret_desc = ""
        cur_cmd_idx = 1
        for cmd in self.cfg_cmds:
            # 发送命令
            self.send(bytes(cmd + "\n", "utf-8"))
            if cmd == "closeWifi":
                # 对关闭wifi做特殊超时处理
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=5)
            else:
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=self.comm_timeout)
            if not ret:
                ret_desc += cmd + " : no data received\r\n"
            elif msg_detail.response != 0x01:
                ret_desc += cmd + " : %02X" % msg_detail.response + "\r\n"
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG,
                                                int(cur_cmd_idx * 100 / len(self.cfg_cmds))))
            cur_cmd_idx += 1
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))

    def exec_filter(self): #clutter filter executor 
        ret_desc = ""
        # 1. 查目标
        targets = []
        self.send(bytes("getTarget\n", "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_TARGET, timeout=self.comm_timeout)
        if ret:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG, 20))
            targets = msg_detail.tags[0].targets
        else:
            ret_desc = "目标获取失败"
            WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))
            return
        # 2. 查配置
        cfgs = ""
        self.send(bytes("getWorkParam\n", "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_PARAM, timeout=self.comm_timeout)
        if ret:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG, 40))
            cfgs = msg_detail.cmds
        else:
            ret_desc = "获取CFG参数失败"
            WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))
            return
        # 3. 改配置
        cmds = cfgs.split("\n")
        new_cmds = []
        for cmd in cmds:
            if 'SceneryParam' in cmd:
                segs = cmd.split(" ")
                if len(segs) == 7:
                    # 老语法，统一到新语法，添加0
                    segs.append("0")
                if segs[-1] == "0":
                    new_cmds.append(" ".join(segs))
                    # 添加杂波点 clutter filter
                    for t in targets:
                        new_cmds.append("SceneryParam %f %f %f %f %f %f 1" %
                                        (t.x + self.filter_region[0], t.x + self.filter_region[1],
                                         t.y + self.filter_region[2], t.y + self.filter_region[3],
                                         t.z + self.filter_region[4], t.z + self.filter_region[5]))
            else:
                new_cmds.append(cmd)
        #
        cur_cmd_idx = 1
        for cmd in new_cmds:
            # 发送命令
            self.send(bytes(cmd + "\n", "utf-8"))
            if cmd == "closeWifi":
                # 对关闭wifi做特殊超时处理
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=5)
            else:
                ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=self.comm_timeout)
            if not ret:
                ret_desc += cmd + " : no data received\r\n"
            elif msg_detail.response != 0x01:
                ret_desc += cmd + " : %02X" % msg_detail.response + "\r\n"
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_CFG,
                                                40 + int(cur_cmd_idx * 60 / len(new_cmds))))
            cur_cmd_idx += 1
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_CFG, ret_desc))

    def update_firmware(self):
        ret_desc = ""
        # 1. 发送ReadyUpdate
        ret = False
        for i in range(5):
            self.send(bytes("ReadyUpdate\n", "utf-8"))
            ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=1)
            if ret and msg_detail.response == 0x21 and msg_detail.crc == 0xcb:
                ret = True
                break
        if not ret:
            ret_desc = "ReadyUpdate命令未收到响应"
        else:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 10))
            # 2. 发送Update
            self.send(bytes("Update\n", "utf-8"))
            ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
            if not ret or msg_detail.response != 0x22 or msg_detail.crc != 0xcc:
                ret_desc = "Update命令未收到响应"
            else:
                WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 20))
                # 3. XModem协议传输文件
                file = open(self.firm_path, "rb")
                file_size = os.path.getsize(self.firm_path)
                packet_num = math.ceil(file_size / WorkerWifi.packet_size)

                def getc(size, timeout=5):
                    cache = bytes()
                    x_time = time.time()
                    while time.time() - x_time < timeout:
                        data = self.recv(size)
                        if data is not None and len(data) > 0:
                            cache += data
                            if len(cache) >= size:
                                return cache[0: size]
                        else:
                            time.sleep(0.005)
                    return None

                def putc(data, timeout=1):
                    return self.send(data)

                def callback_func(total_packets, success_count, error_count):
                    WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM,
                                                        20 + int(success_count * 70 / packet_num)))

                if WorkerWifi.packet_size == 128:
                    xmodem = XMODEM(getc, putc, mode='xmodem')
                else:
                    xmodem = XMODEM(getc, putc, mode='xmodem1k')
                xmodem_ret = xmodem.send(file, timeout=5, callback=callback_func)
                file.close()
                if not xmodem_ret:
                    ret_desc = "XModem传输错误"
                else:
                    # 4. 确认响应
                    ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
                    if not ret or msg_detail.response != 0x23 or msg_detail.crc != 0xcd:
                        ret_desc = "未收到固件更新完成响应"
                    else:
                        WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_FIRM, 100))
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_FIRM, ret_desc))

    def update_sbl(self):
        ret_desc = ""
        # 1. 发送sbl升级请求
        file_size = os.path.getsize(self.sbl_path)
        packet_num = math.ceil(file_size / WorkerWifi.packet_size)
        self.send(bytes("updateSbl %d\n" % file_size, "utf-8"))
        ret, msg_detail = self.get_msg485(CMD_485_OTA, timeout=15)
        if not ret:
            ret_desc = "updateSbl命令未收到响应"
        elif msg_detail.response != 0x01:
            ret_desc = "updateSbl命令响应错误 : %02X" % msg_detail.response
        else:
            WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_SBL, 10))
            # 2. 传输文件
            file = open(self.sbl_path, "rb")
            for i in range(packet_num):
                if i < packet_num - 1:
                    cmd_bytes = file.read(WorkerWifi.packet_size)
                else:
                    cmd_bytes = file.read()
                for j in range(5):
                    self.send(Msg485Send(1, frame_idx=i+1).get_cmd(CMD_485_SBL, cmd_bytes))
                    ret, msg_detail = self.get_msg485(CMD_485_SBL, timeout=5)
                    if not ret:
                        ret_desc = "sbl升级包未收到响应"
                    elif msg_detail.response != 0x01:
                        ret_desc = "sbl升级包响应错误 : %02X" % msg_detail.response
                    else:
                        ret_desc = ""
                        WorkerWifi.queue_progress_rate.put((self.ip_port, PROGRESS_TYPE_SBL, 10 + int((i + 1) * 90 / packet_num)))
                        break
                if ret_desc != "":
                    break
            file.close()
        WorkerWifi.queue_progress_rets.put((self.ip_port, PROGRESS_TYPE_SBL, ret_desc))


class WifiTCPServer(Thread):
    def __init__(self, server):
        """
        构造函数
        """
        super(WifiTCPServer, self).__init__()
        self.server = server

    def end(self):
        """
        停止接收数据
        """
        self.server.shutdown()
        self.server.server_close()
        return True

    def run(self):
        self.server.serve_forever()


class WorkerWifi(WorkerBase):
    """
    网络数据接收器
    """
    # TCP连接字典
    dict_clients = {}
    # 线程共享数据
    queue_msg = Queue()
    queue_progress_rate = Queue()
    queue_progress_rets = Queue()
    queue_radar_disconnect = Queue()
    #
    radar_timeout = 1
    comm_timeout = 0.2
    packet_size = 128
    debug_target = False
    detail_target = False

    def __init__(self, ip, port, radar_timeout=1, comm_timeout=0.2, debug_target=False, detail_target=False, packet_size=128):
        """
        构造函数
        """
        super(WorkerWifi, self).__init__()
        self.ip = ip
        self.port = port
        self.tcp_server = None
        WorkerWifi.debug_target = debug_target
        WorkerWifi.detail_target = detail_target
        WorkerWifi.radar_timeout = radar_timeout
        WorkerWifi.comm_timeout = comm_timeout
        WorkerWifi.packet_size = packet_size

    def query(self, query_desc):
        self.query_desc = query_desc

    def cfg_config(self, cfg_cmds, cfg_filter):
        """
        cfg配置
        :param cfg_cmds:
        :param cfg_filter:
        :return:
        """
        self.cfg_cmds = cfg_cmds.split("\n")
        self.cfg_filter = cfg_filter

    def firm_update(self, firm_path, firm_filter):
        """
        固件更新
        :param firm_path:
        :param firm_filter:
        :return:
        """
        self.firm_path = firm_path
        self.firm_filter = firm_filter

    def sbl_update(self, sbl_path, sbl_filter):
        self.sbl_path = sbl_path
        self.sbl_filter = sbl_filter

    def init(self):
        """
        测试网络是否能够正常连接
        :return:
        """
        try:
            socketserver.TCPServer.allow_reuse_address = True
            server = socketserver.ThreadingTCPServer((self.ip, self.port), WifiTCPHandler)
            self.tcp_server = WifiTCPServer(server)
        except Exception as e:
            print(e)
            return False
        return True

    def kick_client(self, desc):
        if desc in WorkerWifi.dict_clients.keys():
            WorkerWifi.dict_clients[desc].end()

    def kick_all_client(self):
        for desc, client in WorkerWifi.dict_clients.items():
            client.end()

    def reset_descs(self, ids_485):
        return

    def add_485id(self, id_485):
        return

    def del_485id(self, id_485):
        self.kick_client(id_485)

    def run(self):
        self.tcp_server.start()
        while self.is_run:
            try:
                time.sleep(0.005)
                # 消息通知
                while not WorkerWifi.queue_msg.empty():
                    p = WorkerWifi.queue_msg.get()
                    smokesignal.emit('msg_signal', p[0], p[1])
                # 发送cfg配置
                if self.cfg_cmds is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.cfg_filter:
                            tcp_client.cfg_cmds = self.cfg_cmds
                            descs.append(desc)
                    self.cfg_cmds = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_CFG, ";".join(descs)))
                # 杂波滤除 CLUTTER FILTER
                if self.filter_region is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.filter_filter:
                            tcp_client.filter_region = self.filter_region
                            descs.append(desc)
                    self.filter_region = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_CFG, ";".join(descs)))

                # update firmware
                if self.firm_path is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.firm_filter:
                            tcp_client.firm_path = self.firm_path
                            descs.append(desc)
                    self.firm_path = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_FIRM, ";".join(descs)))
                # update firmware
                if self.sbl_path is not None:
                    descs = []
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc in self.sbl_filter:
                            tcp_client.sbl_path = self.sbl_path
                            descs.append(desc)
                    self.sbl_path = None
                    self.queue_progress_rets.put(("", PROGRESS_TYPE_SBL, ";".join(descs)))
                #
                if self.query_desc is not None:
                    for desc, tcp_client in WorkerWifi.dict_clients.items():
                        if desc == self.query_desc:
                            tcp_client.query_desc = self.query_desc
                    self.query_desc = None
                #
                while not WorkerWifi.queue_progress_rate.empty():
                    p = WorkerWifi.queue_progress_rate.get()
                    smokesignal.emit('progress_rate_signal',p[0], p[1], p[2])
                while not WorkerWifi.queue_progress_rets.empty():
                    p = WorkerWifi.queue_progress_rets.get()
                    smokesignal.emit('progress_result_signal',p[0], p[1], p[2])
                # 客户端退出通知
                while not WorkerWifi.queue_radar_disconnect.empty():
                    p = WorkerWifi.queue_radar_disconnect.get()
                    smokesignal.emit('client_exit_signal',p)
            except Exception as e:
                print(e)
        self.kick_all_client()
        self.tcp_server.end()
        #
        WorkerWifi.dict_clients.clear()
        WorkerWifi.queue_msg.queue.clear()
        WorkerWifi.queue_progress_rate.queue.clear()
        WorkerWifi.queue_progress_rets.queue.clear()
        WorkerWifi.queue_radar_disconnect.queue.clear()

# -*- coding: utf-8 -*- Copyright 2020, Ainstein Inc. All Rights Reserved   This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.  You should have received a copy of the GNU General Public License along with this program.  If not, see <https://www.gnu.org/licenses/>.  Contact: <EMAIL> # 心跳检测间隔 # # # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ") # print("send:", end="") # for b in data: #     print("%02x " % b, end="") # print("") # 收到数据即认为雷达存活 # print(datetime.datetime.now().strftime('%H:%M:%S.%f'), end=" ") # print("recv:", end="") # for b in data: #     print("%02x " % b, end="") # print("") # 将新的TCP连接记录到字典 # self.ip_port = self.client_address[0] + ":" + str(self.client_address[1]) # 设置超时时间 # # 主循环 # 心跳检测（获取版本号） # 下发CFG # update firmware # update sbl # 接收目标数据 # 查询 # 从字典删除该TCP连接 # 收消息，有超时时间 # 发送命令 # 对关闭wifi做特殊超时处理 #clutter filter executor # 1. 查目标 # 2. 查配置 # 3. 改配置 # 老语法，统一到新语法，添加0 # 添加杂波点 clutter filter # # 发送命令 # 对关闭wifi做特殊超时处理 # 1. 发送ReadyUpdate # 2. 发送Update # 3. XModem协议传输文件 # 4. 确认响应 # 1. 发送sbl升级请求 # 2. 传输文件 构造函数 停止接收数据 网络数据接收器 # TCP连接字典 # 线程共享数据 # 构造函数 cfg配置         :param cfg_cmds:         :param cfg_filter:         :return: 固件更新         :param firm_path:         :param firm_filter:         :return: 测试网络是否能够正常连接         :return: # 消息通知 # 发送cfg配置 # 杂波滤除 CLUTTER FILTER # update firmware # update firmware # # # 客户端退出通知 #