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