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] # 后天线使用卫星数 前天线当前参与解算的卫星数量 if abs(x_car-float(east))>10 or abs(y_car-float(north))>10: x_car=float(east) y_car=float(north) 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()