You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
WRC/tcp4.py

129 lines
4.6 KiB
Python

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

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()