V0.1

tcp4.py

@@ -0,0 +1,126 @@
+import datetime
+import math
+import socket
+
+import numpy as np
+from loguru import logger
+
+clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+clientSocket.connect(('192.168.43.100', 4002))
+
+# # 发
+# sendData = input('>>')
+# clientSocket.send(sendData.encode('gbk'))
+# 收
+# jishi=0
+# while jishi<= 30:
+
+x = 0
+y = 0
+yaw = 0
+x_car = 0
+y_car = 0
+heading = 0
+east = 0
+north = 0
+beidou = 0
+
+
+def zuo_biao():
+    global yaw, x_car, y_car, east, north, beidou, heading
+    while True:
+        recvData = clientSocket.recv(1024)
+        tcpdata = recvData.decode('gbk')
+        # print(tcpdata)
+        beidou = tcpdata.split(',')
+        # print(beidou)
+        # print(len(beidou))
+        # KSXT = beidou[0]
+        # if KSXT=='$KSXT':
+        KSXT = beidou[0]  # 帧头
+        time0 = beidou[1]  # 时间
+        lon = beidou[2]  # 经度
+        lat = beidou[3]  # 纬度
+        height = beidou[4]  # 海拔
+
+        heading = beidou[5]  # 方位角
+        heading = float(heading)
+        heading = heading + 180
+        if heading > 360:
+            heading = heading - 360
+        if 0 < heading <= 90:
+            yaw = math.radians(90 - float(heading))
+            yaw = round(yaw, 3)
+
+        else:
+            yaw = math.radians(450 - float(heading))
+            yaw = round(yaw, 3)
+        print("yaw", yaw)
+
+        # pitch = beidou[6]  # 俯仰角
+        # tracktrue = beidou[7]  # 速度角
+        # vel = beidou[8]  # 水平速度
+        # roll = beidou[9]  # 横滚  空
+        # pos_qual = beidou[10]  # GNSS定位质量指示符0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
+        # heading_qual = beidou[11]  # GNSS定向质量指示符 0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
+        # solnsvsh = beidou[12]  # 前天线使用卫星数 前天线当前参与解算的卫星数量
+        # solnsvsb = beidou[13]  # 后天线使用卫星数 前天线当前参与解算的卫星数量
+
+        east = beidou[14]  # 东向位置坐标：以基站为原点的地理坐标系下的东向位置，单位：米，小数点后 3 位
+        x_car = np.cos(yaw) * 2.3 + float(east)
+        x_car = round(x_car, 3)
+        # print('x',x_car)
+        north = beidou[15]  # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
+        y_car = -np.sin(yaw) * 2.3 + float(north)
+        y_car = round(y_car, 3)
+        shijian = datetime.datetime.now()
+
+    # logger.add(
+    # 	'x,y记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
+    # logger.debug('北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(east), str(north), str(x_car),
+    # 			 str(y_car), str(beidou[5]), str(heading))
+    # up = beidou[16]  # 天向位置坐标：以基站为原点的地理坐标系下的天顶向位置，单位：米，小数点后 3 位
+    # eastvel = beidou[17]  # 东向速度：地理坐标系下的东向速度，小数点后 3位，单位：Km/h(如无为空)
+    # northvel = beidou[18]  # 北向速度：地理坐标系下的北向速度，小数点后 3位，单位：Km/h(如无为空)
+    # uphvel = beidou[19]  # 天向速度：地理坐标系下的天顶向速度，小数点后3 位，单位：Km/h(如无为空)
+    # unknown = beidou[20]
+    # check = beidou[21]  # 异或校验（十六进制字符串，从帧头开始校验）
+    # east = float(east)
+    # north = float(north)
+
+# print('接收时间为:',datetime.datetime.now())
+# print('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s; ' % ( heading, east, north))
+# print('\n')
+
+# shijian=datetime.datetime.now()
+
+# file = open('1.txt', 'a')
+# file.write(str('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s; ' % (heading, east, north)))
+# file.write('\n')
+# jishi=jishi+1
+# time.sleep(0.05)
+
+# else:
+#     print('未接收到定位消息，重新连接')
+#     clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+#     clientSocket.connect(('192.168.43.100', 4002))
+#     # clientSocket.close()
+
+# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
+# if 0.2>=d >= 0.1:
+# 	print('大于', d)
+# 	print('\n')
+# 	file = open('1.txt', 'a')
+# 	file.write(str('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s;与上一点的距离为%s ' % (heading, x, y,d)))
+# 	file.write('\n')
+# 	x = east
+# 	y = north
+# 	print(x, y)
+# else:
+# 	print('不在范围', d)
+# 	print('\n')
+# 	if x==0 and y==0:
+# 		x = east
+# 		y = north
+# time.sleep(0.05)
+# zuo_biao()