def __init__(self, com): self.roll = 0 self.pitch = 0 self.yaw = 0 self.GYRO_COM = com self.GYRO_REC_BUF_LEN = (11 * 4) self.com_gyro = SerialPortCommunication(self.GYRO_COM, 9600, 0.5)
class Laser(object):
def __init__(self, com):
self.distance = 0
self.LASER_COM = com
self.com_laser = SerialPortCommunication(self.LASER_COM, 9600, 0.01)
def laser_read_data(self, LASER_REC_BUF_LEN):
laser_rec_buf = self.com_laser.read_size(LASER_REC_BUF_LEN) # bytes
if laser_rec_buf != b'':
return laser_rec_buf
def get_distance(self, laser_rec_buf):
# print(laser_rec_buf)
# SUCC: b'\x80\x06\x83000.212\xa4'
# ERROR: b'\x80\x06\x83ERR--15N'
# 切片有效数据
# addr_sensor 06 83 3X 3X 3X 2E 3X 3X 3X CS
distance = laser_rec_buf[3:10]
# print(distance) # b'000.103'
if distance == b'ERR--15':
print("--LASER READ ERROR--")
else:
self.distance = float(distance)
# print("distance: ", self.distance)
return self.distance
class Gyro3(object):
def __init__(self, com):
self.roll = 0
self.pitch = 0
self.yaw = 0
self.GYRO_COM = com
self.GYRO_REC_BUF_LEN = (11 * 4)
self.com_gyro = SerialPortCommunication(self.GYRO_COM, 9600, 0.5)
def get_angle(self):
gyro_rec_buf = self.com_gyro.read_size(self.GYRO_REC_BUF_LEN)
target_index = gyro_rec_buf.find(0x53) # 数据头2角度输出
if target_index != (-1):
if gyro_rec_buf[target_index - 1] == 0x55: # 数据头1
data = gyro_rec_buf[(target_index - 1):(target_index + 10)]
self.roll = int(((data[3] << 8) | data[2])) / 32768 * 180
self.pitch = int(((data[5] << 8) | data[4])) / 32768 * 180
self.yaw = int(((data[7] << 8) | data[6])) / 32768 * 180
print("roll:", self.roll)
print("pitch:", self.pitch)
print("yaw:", self.yaw)
else:
print("header 1 error")
return -1
else:
print("header 2 error")
return -1
class Gyro2(object):
def __init__(self, com):
self.roll = 0
self.pitch = 0
self.GYRO_COM = com
self.GYRO_REC_BUF_LEN = (11 * 3)
self.com_gyro = SerialPortCommunication(self.GYRO_COM, 115200, 0.5)
def get_angle(self):
gyro_rec_buf = self.com_gyro.read_size(self.GYRO_REC_BUF_LEN)
if gyro_rec_buf:
RollH = gyro_rec_buf[3]
RollL = gyro_rec_buf[4]
PitchH = gyro_rec_buf[5]
PitchL = gyro_rec_buf[6]
if gyro_rec_buf[0] == 0x50 and gyro_rec_buf[1] == 0x03:
self.roll = int(((RollH << 8) | RollL)) / 32768 * 180
self.pitch = int(((PitchH << 8) | PitchL)) / 32768 * 180
self.roll = round(self.roll, 2) # 保存2位小数
self.pitch = round(self.pitch, 2)
return self.roll, self.pitch
else:
self.roll = 0
self.pitch = 0
print("Big arm gyro data is null")
return self.roll, self.pitch
def _4gProcess():
gl.gl_init()
global COM_ID_4G, TYPE_HEART, TYPE_SEND, com_4g, diggerId
if SWITCH_DEVICE:
COM_ID_4G = "com13"
else:
COM_ID_4G = "com4"
TYPE_HEART = 1 # 消息类型。1：心跳，2：上报
TYPE_SEND = 2
diggerId = 566609996553388032
com_4g = SerialPortCommunication(COM_ID_4G, 115200, 0.1)
# 上报消息socket
sk_send = socket.socket()
sk_send.bind(("127.0.0.1", 14612))
sk_send.listen()
conn_send, addr_send = sk_send.accept()
sk_send.setblocking(False)
# 接收任务socket
sk_recv_tsk = socket.socket()
sk_recv_tsk.connect(("127.0.0.1", 9000))
_4g_recv_thread = threading.Thread(target=_4RecFunc, daemon=True).start()
_4g_send_thread = threading.Thread(target=_4SendFunc, daemon=True).start()
""" 4g 主线程 """
while True:
time.sleep(0.2)
my_lock.p4g_rec_main_lock.acquire()
_4g_recv_flg = gl.get_value("_4g_recv_flg")
my_lock.p4g_rec_main_lock.release()
if _4g_recv_flg:
my_lock.p4g_rec_main_lock.acquire()
gl.set_value("_4g_recv_flg", False)
_4g_data_dict = {
"_4g_data_valid_flg": gl.get_value("_4g_data_valid_flg"),
"g_start_x_list": gl.get_value('g_start_x_list'),
"g_start_y_list": gl.get_value('g_start_y_list'),
"g_start_h_list": gl.get_value('g_start_h_list'),
"g_start_w_list": gl.get_value('g_start_w_list'),
"g_end_x_list": gl.get_value('g_end_x_list'),
"g_end_y_list": gl.get_value('g_end_y_list'),
"g_end_h_list": gl.get_value('g_end_h_list'),
"g_end_w_list": gl.get_value('g_end_w_list'),
}
my_lock.p4g_rec_main_lock.release()
_4g_data_bytes = json.dumps(_4g_data_dict)
sk_recv_tsk.send(bytes(_4g_data_bytes.encode('utf-8')))
# 上报
_4g_send_bytes = conn_send.recv(1024)
if _4g_send_bytes:
_4g_send_dict = json.loads(_4g_send_bytes)
my_lock.p4g_main_send_lock.acquire()
gl.set_value("_4g_send_dict", _4g_send_dict)
my_lock.p4g_main_send_lock.release()
def gpsThreadFunc():
log.debug("gpsThreadFunc 线程启动 ...")
if SWITCH_DEVICE:
GPS_COM = "com1"
else:
GPS_COM = "com11"
GPS_REC_BUF_LEN = 138
gps_msg_switch = LatLonAlt()
gps_com = SerialPortCommunication(GPS_COM, 115200, 0.05)
x_filter_list = [] # 初始化滤波列表
y_filter_list = []
h_filter_list = []
yaw_filter_list = []
filter_len = 7
""" 等待数据准备 """
while True:
gps_data = GPSINSData()
gps_rec_buffer = []
time.sleep(0.001)
gps_com.rec_data(gps_rec_buffer, GPS_REC_BUF_LEN) # int
if gps_data.gps_msg_analysis(gps_rec_buffer):
gps_msg_switch.latitude, gps_msg_switch.longitude, gps_msg_switch.altitude, \
gps_msg_switch.yaw, gps_msg_switch.yaw_state = gps_data.gps_typeswitch()
y, x = LatLon2XY(gps_msg_switch.latitude, gps_msg_switch.longitude)
h = gps_msg_switch.altitude
x = round(x, 3)
y = round(y, 3)
h = round(h, 3)
yaw = round(gps_msg_switch.yaw, 3)
x_filter_list.append(x)
y_filter_list.append(y)
h_filter_list.append(h)
yaw_filter_list.append(yaw)
# 中值滤波
x_mid = mid_filter(x_filter_list)
y_mid = mid_filter(y_filter_list)
h_mid = mid_filter(h_filter_list)
yaw_mid = mid_filter(yaw_filter_list)
if len(x_filter_list) == filter_len:
gps_x = x_mid
gps_y = y_mid
gps_h = h_mid
gps_yaw = yaw_mid
gps_data_valid_flg = True
# 设置全局变量
my_lock.pSor_gps_main_lock.acquire()
gl.set_value("gps_data_valid_flg", gps_data_valid_flg)
gl.set_value("gps_x", gps_x)
gl.set_value("gps_y", gps_y)
gl.set_value("gps_h", gps_h)
gl.set_value("gps_yaw", gps_yaw)
my_lock.pSor_gps_main_lock.release()
break
else:
print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx_filter_list:%s\n" %
x_filter_list)
print("yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy_filter_list:%s\n" %
y_filter_list)
print("hhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh_filter_list:%s\n" %
h_filter_list)
else:
print("！！！！！！gps准备过程解析错误！！！！！！\n")
time.sleep(0.1)
""" 正常流程 """
while True:
gps_data = GPSINSData()
gps_rec_buffer = []
if gps_com.com.is_open:
gps_is_open_led = True # gps串口是否打开
gps_com.rec_data(gps_rec_buffer, GPS_REC_BUF_LEN) # int
if gps_data.gps_msg_analysis(gps_rec_buffer):
gps_msg_switch.latitude, gps_msg_switch.longitude, gps_msg_switch.altitude, \
gps_msg_switch.yaw, gps_msg_switch.yaw_state = gps_data.gps_typeswitch()
"""经纬度转高斯坐标"""
y, x = LatLon2XY(gps_msg_switch.latitude,
gps_msg_switch.longitude)
h = gps_msg_switch.altitude
x = round(x, 3)
y = round(y, 3)
h = round(h, 3)
yaw = round(gps_msg_switch.yaw, 3)
x_filter_list.append(x)
y_filter_list.append(y)
h_filter_list.append(h)
yaw_filter_list.append(yaw)
x_filter_list.pop(0)
y_filter_list.pop(0)
h_filter_list.pop(0)
yaw_filter_list.pop(0)
# 均值滤波
x_mid = mid_filter(x_filter_list)
y_mid = mid_filter(y_filter_list)
h_mid = mid_filter(h_filter_list)
yaw_mid = mid_filter(yaw_filter_list)
# 设置全局变量
my_lock.pSor_gps_main_lock.acquire()
gl.set_value("gps_x", x_mid)
gl.set_value("gps_y", y_mid)
gl.set_value("gps_h", h_mid)
gl.set_value("gps_yaw", yaw_mid)
my_lock.pSor_gps_main_lock.release()
# print("平面坐标: %s\t %s\t %s\t 偏航角度 %s " % (x, y, h, yaw))
else:
gps_is_open_led = False
my_lock.gpsLedLock.acquire()
gl.set_value("gps_is_open_led", gps_is_open_led)
my_lock.gpsLedLock.release()
time.sleep(0.1)
def _485ThreadFunc():
log.debug("_485ThreadFunc 线程启动 ...")
if SWITCH_DEVICE:
BUS_COM = "com5"
else:
BUS_COM = "com7"
GyroRecBufLen = (11 * 3)
gyroChassisaddr_sensor = 0x50
gyroBigArmaddr_sensor = 0x51
gyroLittleArmaddr_sensor = 0x52
bus485 = SerialPortCommunication(BUS_COM, 115200, 0.005)
gyroChassisReadCmd = [
gyroChassisaddr_sensor, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x99, 0x86
]
gyroBigArmReadCmd = [
gyroBigArmaddr_sensor, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x98, 0x57
]
gyroLittleReadCmd = [
gyroLittleArmaddr_sensor, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x98, 0x64
]
""" 准备工作 """
while True:
gyroChassisAngle = []
gyroBigArmAngle = []
gyroLittleArmAngle = []
chassis_data_ready = False
big_data_ready = False
little_data_ready = False
# 全局变量初始化
_485_data_valid_flg = False
roll_2_di_pan = 0
pitch_2_di_pan = 0
roll_2_da_bi = 0
roll_2_xiao_bi = 0
chassisLen = bus485.send_data(gyroChassisReadCmd)
time.sleep(0.001)
if chassisLen == len(gyroChassisReadCmd):
gyroChassisRecBuf = bus485.read_size(GyroRecBufLen)
if gyroChassisRecBuf != b'':
gyroChassisAngle = gyroDataAnalysis(gyroChassisRecBuf,
gyroChassisaddr_sensor)
chassis_data_ready = True
roll_2_di_pan = gyroChassisAngle[0]
pitch_2_di_pan = gyroChassisAngle[1]
else:
print("准备流程--地盘--接收数组为空\n")
else:
print("准备流程--地盘--地址发送失败\n")
time.sleep(0.001)
bigArmLen = bus485.send_data(gyroBigArmReadCmd)
time.sleep(0.001)
if bigArmLen == len(gyroBigArmReadCmd):
gyroBigArmRecBuf = bus485.read_size(GyroRecBufLen)
# TODO：判断条件
if gyroBigArmRecBuf != b'':
gyroBigArmAngle = gyroDataAnalysis(gyroBigArmRecBuf,
gyroBigArmaddr_sensor)
big_data_ready = True
roll_2_da_bi = gyroBigArmAngle[0]
else:
print("准备流程--大臂--接收数组为空\n")
else:
print("准备流程--大臂--地址发送失败\n")
time.sleep(0.001)
littleArmLen = bus485.send_data(gyroLittleReadCmd)
time.sleep(0.001)
if littleArmLen == len(gyroLittleReadCmd):
gyroLittleArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroLittleArmRecBuf != b'':
gyroLittleArmAngle = gyroDataAnalysis(
gyroLittleArmRecBuf, gyroLittleArmaddr_sensor)
little_data_ready = True
roll_2_xiao_bi = gyroLittleArmAngle[0]
else:
print("准备流程--小臂--接收数组为空\n")
else:
print("准备流程--小臂--地址发送失败\n")
# print("==============================485准备：", chassis_data_ready, big_data_ready, little_data_ready)
if chassis_data_ready and big_data_ready and little_data_ready:
_485_data_valid_flg = True
# 设置全局变量
my_lock.pSor_gyro_main_lock.acquire()
gl.set_value("roll_2_di_pan", roll_2_di_pan)
gl.set_value("pitch_2_di_pan", pitch_2_di_pan)
gl.set_value("roll_2_da_bi", roll_2_da_bi)
gl.set_value("roll_2_xiao_bi", roll_2_xiao_bi)
gl.set_value("_485_data_valid_flg", _485_data_valid_flg)
my_lock.pSor_gyro_main_lock.release()
break
else:
print(
"CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCgyroChassisAngle:%s\n"
% gyroChassisAngle)
print(
"BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBgyroBigArmAngle:%s\n"
% gyroBigArmAngle)
print(
"LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLgyroLittleArmAngle:%s\n"
% gyroLittleArmAngle)
time.sleep(0.1)
""" 正常流程 """
while True:
roll_2_di_pan = 0
pitch_2_di_pan = 0
roll_2_da_bi = 0
roll_2_xiao_bi = 0
gyroChassisAngle = []
gyroBigArmAngle = []
gyroLittleArmAngle = []
time.sleep(0.001)
if bus485.com.is_open:
chassisLen = bus485.send_data(gyroChassisReadCmd)
time.sleep(0.001)
if chassisLen == len(gyroChassisReadCmd):
gyroChassisRecBuf = bus485.read_size(GyroRecBufLen)
if gyroChassisRecBuf != b'':
gyroChassisAngle = gyroDataAnalysis(
gyroChassisRecBuf, gyroChassisaddr_sensor)
# print("===========================地盘角度：", gyroChassisAngle)
gyroLedChassis = True
roll_2_di_pan = gyroChassisAngle[0]
pitch_2_di_pan = gyroChassisAngle[1]
else:
gyroLedChassis = False
print("工作流程--底盘--接收数组为空\n")
else:
gyroLedChassis = False
print("工作流程--底盘--发送失败\n")
time.sleep(0.001)
bigArmLen = bus485.send_data(gyroBigArmReadCmd)
time.sleep(0.001)
if bigArmLen == len(gyroBigArmReadCmd):
gyroBigArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroBigArmRecBuf != b'':
gyroBigArmAngle = gyroDataAnalysis(gyroBigArmRecBuf,
gyroBigArmaddr_sensor)
# print("===========================大臂角度：", gyroBigArmAngle)
gyroBigLed = True
roll_2_da_bi = gyroBigArmAngle[0]
else:
gyroBigLed = False
print("工作流程--大臂--接收数组为空\n")
else:
gyroBigLed = False
# gl.set_value("gyroBigLed", False)
print("工作流程--大臂--发送失败\n")
time.sleep(0.001)
littleArmLen = bus485.send_data(gyroLittleReadCmd)
time.sleep(0.001)
if littleArmLen == len(gyroLittleReadCmd):
gyroLittleArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroLittleArmRecBuf != b'':
gyroLittleArmAngle = gyroDataAnalysis(
gyroLittleArmRecBuf, gyroLittleArmaddr_sensor)
# print("===========================小臂角度：", gyroLittleArmAngle)
gyroLittleLed = True
roll_2_xiao_bi = gyroLittleArmAngle[0]
else:
gyroLittleLed = False
print("工作流程--小臂--接收数组为空\n")
else:
gyroLittleLed = False
print("工作流程--小臂--发送失败\n")
if gyroChassisAngle and gyroBigArmAngle and gyroLittleArmAngle:
# 设置全局变量
my_lock.pSor_gyro_main_lock.acquire()
gl.set_value("roll_2_di_pan", roll_2_di_pan)
gl.set_value("pitch_2_di_pan", pitch_2_di_pan)
gl.set_value("roll_2_da_bi", roll_2_da_bi)
gl.set_value("roll_2_xiao_bi", roll_2_xiao_bi)
my_lock.pSor_gyro_main_lock.release()
else:
gyroLedChassis = False
gyroBigLed = False
gyroLittleLed = False
my_lock.gyroLedLock.acquire()
gl.set_value("gyroLedChassis", gyroLedChassis)
gl.set_value("gyroBigLed", gyroBigLed)
gl.set_value("gyroLittleLed", gyroLittleLed)
my_lock.gyroLedLock.release()
time.sleep(0.01)
def bus485ThreadFunc():
# BUS_COM = "com7"
BUS_COM = "com5"
GyroRecBufLen = (11 * 3)
gyroChassisAddr = 0x50
gyroBigArmAddr = 0x51
gyroLittleArmAddr = 0x52
bus485 = SerialPortCommunication(BUS_COM, 115200, 0.005)
gyroChassisReadCmd = [gyroChassisAddr, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x99, 0x86]
gyroBigArmReadCmd = [gyroBigArmAddr, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x98, 0x57]
gyroLittleReadCmd = [gyroLittleArmAddr, 0x03, 0x00, 0x3d, 0x00, 0x03, 0x98, 0x64]
""" 准备工作 """
while True:
gyroChassisAngle = []
gyroBigArmAngle = []
gyroLittleArmAngle = []
chassis_data_ready = False
big_data_ready = False
little_data_ready = False
# 全局变量初始化
_485_data_valid_flg = False
roll_2_di_pan = 0
pitch_2_di_pan = 0
roll_2_da_bi = 0
roll_2_xiao_bi = 0
chassisLen = bus485.send_data(gyroChassisReadCmd)
time.sleep(0.001)
if chassisLen == len(gyroChassisReadCmd):
gyroChassisRecBuf = bus485.read_size(GyroRecBufLen)
if gyroChassisRecBuf != b'':
gyroChassisAngle = gyroDataAnalysis(gyroChassisRecBuf, gyroChassisAddr)
chassis_data_ready = True
roll_2_di_pan = gyroChassisAngle[0]
pitch_2_di_pan = gyroChassisAngle[1]
else:
print("准备流程--地盘--接收数组为空\n")
else:
print("准备流程--地盘--地址发送失败\n")
time.sleep(0.001)
bigArmLen = bus485.send_data(gyroBigArmReadCmd)
time.sleep(0.001)
if bigArmLen == len(gyroBigArmReadCmd):
gyroBigArmRecBuf = bus485.read_size(GyroRecBufLen)
# TODO：判断条件
if gyroBigArmRecBuf != b'':
gyroBigArmAngle = gyroDataAnalysis(gyroBigArmRecBuf, gyroBigArmAddr)
big_data_ready = True
roll_2_da_bi = gyroBigArmAngle[0]
else:
print("准备流程--大臂--接收数组为空\n")
else:
print("准备流程--大臂--地址发送失败\n")
time.sleep(0.001)
littleArmLen = bus485.send_data(gyroLittleReadCmd)
time.sleep(0.001)
if littleArmLen == len(gyroLittleReadCmd):
gyroLittleArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroLittleArmRecBuf != b'':
gyroLittleArmAngle = gyroDataAnalysis(gyroLittleArmRecBuf, gyroLittleArmAddr)
little_data_ready = True
roll_2_xiao_bi = gyroLittleArmAngle[0]
else:
print("准备流程--小臂--接收数组为空\n")
else:
print("准备流程--小臂--地址发送失败\n")
# print("==============================485准备：", chassis_data_ready, big_data_ready, little_data_ready)
if chassis_data_ready and big_data_ready and little_data_ready:
_485_data_valid_flg = True
# 设置全局变量
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--in" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Calculator485Lock.acquire()
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--ing" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
gl.set_value("roll_2_di_pan", roll_2_di_pan)
gl.set_value("pitch_2_di_pan", pitch_2_di_pan)
gl.set_value("roll_2_da_bi", roll_2_da_bi)
gl.set_value("roll_2_xiao_bi", roll_2_xiao_bi)
gl.set_value("_485_data_valid_flg", _485_data_valid_flg)
my_lock.Calculator485Lock.release()
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--out" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
break
else:
print("CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCgyroChassisAngle:%s\n" % gyroChassisAngle)
print("BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBgyroBigArmAngle:%s\n" % gyroBigArmAngle)
print("LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLgyroLittleArmAngle:%s\n" % gyroLittleArmAngle)
time.sleep(0.1)
""" 正常流程 """
while True:
roll_2_di_pan = 0
pitch_2_di_pan = 0
roll_2_da_bi = 0
roll_2_xiao_bi = 0
gyroChassisAngle = []
gyroBigArmAngle = []
gyroLittleArmAngle = []
time.sleep(0.001)
if bus485.com.is_open:
chassisLen = bus485.send_data(gyroChassisReadCmd)
time.sleep(0.001)
if chassisLen == len(gyroChassisReadCmd):
gyroChassisRecBuf = bus485.read_size(GyroRecBufLen)
if gyroChassisRecBuf != b'':
gyroChassisAngle = gyroDataAnalysis(gyroChassisRecBuf, gyroChassisAddr)
# print("===========================地盘角度：", gyroChassisAngle)
gyroLedChassis = True
roll_2_di_pan = gyroChassisAngle[0]
pitch_2_di_pan = gyroChassisAngle[1]
else:
gyroLedChassis = False
print("工作流程--底盘--接收数组为空\n")
else:
gyroLedChassis = False
print("工作流程--底盘--发送失败\n")
time.sleep(0.001)
bigArmLen = bus485.send_data(gyroBigArmReadCmd)
time.sleep(0.001)
if bigArmLen == len(gyroBigArmReadCmd):
gyroBigArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroBigArmRecBuf != b'':
gyroBigArmAngle = gyroDataAnalysis(gyroBigArmRecBuf, gyroBigArmAddr)
# print("===========================大臂角度：", gyroBigArmAngle)
gyroBigLed = True
roll_2_da_bi = gyroBigArmAngle[0]
else:
gyroBigLed = False
print("工作流程--大臂--接收数组为空\n")
else:
gyroBigLed = False
# gl.set_value("gyroBigLed", False)
print("工作流程--大臂--发送失败\n")
time.sleep(0.001)
littleArmLen = bus485.send_data(gyroLittleReadCmd)
time.sleep(0.001)
if littleArmLen == len(gyroLittleReadCmd):
gyroLittleArmRecBuf = bus485.read_size(GyroRecBufLen)
if gyroLittleArmRecBuf != b'':
gyroLittleArmAngle = gyroDataAnalysis(gyroLittleArmRecBuf, gyroLittleArmAddr)
# print("===========================小臂角度：", gyroLittleArmAngle)
gyroLittleLed = True
roll_2_xiao_bi = gyroLittleArmAngle[0]
else:
gyroLittleLed = False
print("工作流程--小臂--接收数组为空\n")
else:
gyroLittleLed = False
print("工作流程--小臂--发送失败\n")
if gyroChassisAngle and gyroBigArmAngle and gyroLittleArmAngle:
# 设置全局变量
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--in" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Calculator485Lock.acquire()
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--ing" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
gl.set_value("roll_2_di_pan", roll_2_di_pan)
gl.set_value("pitch_2_di_pan", pitch_2_di_pan)
gl.set_value("roll_2_da_bi", roll_2_da_bi)
gl.set_value("roll_2_xiao_bi", roll_2_xiao_bi)
my_lock.Calculator485Lock.release()
with open("lock_log.txt", "a") as file:
file.write("485线程--calu_485--out" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
else:
gyroLedChassis = False
gyroBigLed = False
gyroLittleLed = False
with open("lock_log.txt", "a") as file:
file.write("485线程--gyrolec--in" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.gyroLedLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("485线程--gyrolec--ing" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
gl.set_value("gyroLedChassis", gyroLedChassis)
gl.set_value("gyroBigLed", gyroBigLed)
gl.set_value("gyroLittleLed", gyroLittleLed)
my_lock.gyroLedLock.release()
with open("lock_log.txt", "a") as file:
file.write("485线程--gyrolec--out" + "\t" + datetime.datetime.now().strftime('%H:%M:%S') + "\n")
time.sleep(0.01)
print("！！！！！485线程！！！！！%s\n" % datetime.datetime.now().strftime('%H:%M:%S.%f'))
def __init__(self, com):
self.distance = 0
self.LASER_COM = com
self.com_laser = SerialPortCommunication(self.LASER_COM, 9600, 0.01)
def thread_gps_func():
# GPS_COM = "com11"
GPS_COM = "com1"
GPS_REC_BUF_LEN = 138
gps_msg_switch = LatLonAlt()
gps_com = SerialPortCommunication(GPS_COM, 115200, 0.05)
x_filter_list = [] # 初始化滤波列表
y_filter_list = []
h_filter_list = []
yaw_filter_list = []
filter_len = 7
""" 等待数据准备 """
while True:
gps_data = GPSINSData()
gps_rec_buffer = []
time.sleep(0.001)
gps_com.rec_data(gps_rec_buffer, GPS_REC_BUF_LEN) # int
if gps_data.gps_msg_analysis(gps_rec_buffer):
gps_msg_switch.latitude, gps_msg_switch.longitude, gps_msg_switch.altitude, \
gps_msg_switch.yaw, gps_msg_switch.yaw_state = gps_data.gps_typeswitch()
y, x = LatLon2XY(gps_msg_switch.latitude, gps_msg_switch.longitude)
h = gps_msg_switch.altitude
x = round(x, 3)
y = round(y, 3)
h = round(h, 3)
yaw = round(gps_msg_switch.yaw, 3)
x_filter_list.append(x)
y_filter_list.append(y)
h_filter_list.append(h)
yaw_filter_list.append(yaw)
# 中值滤波
x_mid = mid_filter(x_filter_list)
y_mid = mid_filter(y_filter_list)
h_mid = mid_filter(h_filter_list)
yaw_mid = mid_filter(yaw_filter_list)
if len(x_filter_list) == filter_len:
gps_x = x_mid
gps_y = y_mid
gps_h = h_mid
gps_yaw = yaw_mid
gps_data_valid_flg = True
# 设置全局变量
with open("lock_log.txt", "a") as file:
file.write("gps线程" + "\t" + "gps_calc_lock" + "\t" + "in" +
"\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
my_lock.GpsCalculatorLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("gps线程" + "\t" + "gps_calc_lock" + "\t" +
"ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
gl.set_value("gps_data_valid_flg", gps_data_valid_flg)
gl.set_value("gps_x", gps_x)
gl.set_value("gps_y", gps_y)
gl.set_value("gps_h", gps_h)
gl.set_value("gps_yaw", gps_yaw)
my_lock.GpsCalculatorLock.release()
with open("lock_log.txt", "a") as file:
file.write("gps线程" + "\t" + "gps_calc_lock" + "\t" +
"out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
break
else:
print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx_filter_list:%s\n" %
x_filter_list)
print("yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy_filter_list:%s\n" %
y_filter_list)
print("hhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh_filter_list:%s\n" %
h_filter_list)
else:
print("！！！！！！gps准备过程解析错误！！！！！！\n")
time.sleep(0.1)
""" 正常流程 """
while True:
gps_data = GPSINSData()
gps_rec_buffer = []
if gps_com.com.is_open:
gps_is_open_led = True # gps串口是否打开
gps_com.rec_data(gps_rec_buffer, GPS_REC_BUF_LEN) # int
if gps_data.gps_msg_analysis(gps_rec_buffer):
gps_msg_switch.latitude, gps_msg_switch.longitude, gps_msg_switch.altitude, \
gps_msg_switch.yaw, gps_msg_switch.yaw_state = gps_data.gps_typeswitch()
"""经纬度转高斯坐标"""
y, x = LatLon2XY(gps_msg_switch.latitude,
gps_msg_switch.longitude)
h = gps_msg_switch.altitude
x = round(x, 3)
y = round(y, 3)
h = round(h, 3)
yaw = round(gps_msg_switch.yaw, 3)
x_filter_list.append(x)
y_filter_list.append(y)
h_filter_list.append(h)
yaw_filter_list.append(yaw)
x_filter_list.pop(0)
y_filter_list.pop(0)
h_filter_list.pop(0)
yaw_filter_list.pop(0)
# 均值滤波
x_mid = mid_filter(x_filter_list)
y_mid = mid_filter(y_filter_list)
h_mid = mid_filter(h_filter_list)
yaw_mid = mid_filter(yaw_filter_list)
# 设置全局变量
with open("lock_log.txt", "a") as file:
file.write("gps线程--gps_calc--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
'\n')
my_lock.GpsCalculatorLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("gps线程--gps_calc--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
'\n')
gl.set_value("gps_x", x_mid)
gl.set_value("gps_y", y_mid)
gl.set_value("gps_h", h_mid)
gl.set_value("gps_yaw", yaw_mid)
my_lock.GpsCalculatorLock.release()
with open("lock_log.txt", "a") as file:
file.write("gps线程--gps_calc--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
'\n')
else:
gps_is_open_led = False
with open("lock_log.txt", "a") as file:
file.write(
str("gps线程--gps_led--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n'))
my_lock.gpsLedLock.acquire()
with open("lock_log.txt", "a") as file:
file.write(
str("gps线程--gps_led--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n'))
gl.set_value("gps_is_open_led", gps_is_open_led)
my_lock.gpsLedLock.release()
with open("lock_log.txt", "a") as file:
file.write(
str("gps线程--gps_led--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n'))
time.sleep(0.1)
print("！！！！！gps线程！！！！！%s\n" %
datetime.datetime.now().strftime('%H:%M:%S'))
def thread_4g_func():
TYPE_HEART = 1 # 消息类型。1：心跳，2：上报
TYPE_SEND = 2
# TODO ： 接
# COM_ID_4G = "com4"
COM_ID_4G = "com13"
com_4g = SerialPortCommunication(COM_ID_4G, 115200, 0.1)
diggerId = 566609996553388032
# 全局变量初始化
_4g_data_valid_flg = False
""" 间隔5秒发送一次心跳 """
heart = Heart(TYPE_HEART, diggerId)
heart.send_heart_msg(com_4g)
start = datetime.datetime.now().replace(minute=0, second=0, microsecond=0)
minute = TimeInterval(start, 5, heart.send_heart_msg, [com_4g])
minute.start()
minute.cancel()
while True:
_4g_recv_flg = False
rec_buf = com_4g.read_line()
# print("==============recv buf %s \n" % rec_buf)
time.sleep(0.001)
if rec_buf != b'':
_4g_recv_flg = True
else:
print(
"\n --4g未接收到任务+++++++++++++++++++++++++++++++++++++++++++ \n")
if _4g_recv_flg:
rec_buf_dict = task_switch_dict(rec_buf) # 转成字典格式
# 保存xyhw列表
sx_list, sy_list, sh_list, sw_list, ex_list, ey_list, eh_list, ew_list = get_xyhw(
rec_buf_dict)
if sx_list and sy_list and sh_list and sw_list and ex_list and ey_list and eh_list and ew_list:
_4g_data_valid_flg = True
# 设置全局变量
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
my_lock.Win4gLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
gl.set_value('g_start_x_list', sx_list)
gl.set_value('g_start_y_list', sy_list)
gl.set_value('g_start_h_list', sh_list)
gl.set_value('g_start_w_list', sw_list)
gl.set_value('g_end_x_list', ex_list)
gl.set_value('g_end_y_list', ey_list)
gl.set_value('g_end_h_list', eh_list)
gl.set_value('g_end_w_list', ew_list)
gl.set_value("_4g_data_valid_flg", _4g_data_valid_flg)
my_lock.Win4gLock.release()
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') +
"\n")
else:
print("\n--任务解析失败--\n")
# 发送o点x,y,h,w
with open("lock_log.txt", "a") as file:
file.write("4g线程--calc_4g--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Calc4gLock.acquire()
o_min_flg = gl.get_value("o_min_flg")
with open("lock_log.txt", "a") as file:
file.write("4g线程--calc_4g--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
o_x_4g = gl.get_value("x_send") # mm
o_y_4g = gl.get_value("y_send")
o_h_4g = gl.get_value("h_send")
my_lock.Calc4gLock.release()
with open("lock_log.txt", "a") as file:
file.write("4g线程--calc_4g--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
with open("lock_log.txt", "a") as file:
file.write("4g线程--win4g--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Win4gLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("4g线程--win4g--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
o_w_flg = gl.get_value("o_w_flg")
o_w_4g = gl.get_value("o_w_4g") # Win4gLock
my_lock.Win4gLock.release()
with open("lock_log.txt", "a") as file:
file.write("4g线程--win4g--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
if o_min_flg and o_w_flg:
send = SendMessage(TYPE_SEND, diggerId, round(o_x_4g / 1000, 3),
round(o_y_4g / 1000, 3),
round(o_h_4g / 1000, 3), o_w_4g) # m
send_msg_json = send.switch_to_json()
com_4g.send_data(send_msg_json.encode('utf-8'))
com_4g.send_data('\n'.encode('utf-8'))
with open("lock_log.txt", "a") as file:
file.write("4g_线程--calc_4g--in" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n')
my_lock.Calc4gLock.acquire()
with open("lock_log.txt", "a") as file:
file.write("4g_线程--calc_4g--ing" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n')
gl.set_value("o_min_flg", False)
my_lock.Calc4gLock.release()
with open("lock_log.txt", "a") as file:
file.write("4g_线程--calc_4g--out" +
datetime.datetime.now().strftime('%H:%M:%S') + '\n')
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--in" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Win4gLock.acquire()
gl.set_value("o_w_flg", False)
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--ing" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
my_lock.Win4gLock.release()
with open("lock_log.txt", "a") as file:
file.write("4g线程--win_4g--out" + "\t" +
datetime.datetime.now().strftime('%H:%M:%S') + "\n")
time.sleep(0.2)
print("！！！！！4g线程！！！！！%s\n" %
datetime.datetime.now().strftime('%H:%M:%S'))