问题描述
我正在尝试使用树莓派将NTRIP RTCM校正注入M8P GPS模块中。 GPS模块通过串口(/ dev / serial0)连接。我收到rtcm校正数据包,但是如何将其转发到GPS模块?我尝试使用serial.write(data),但是它什么也没做。从RaspBerry Pi将RTCM校正数据注入GPS的正确方法是什么?我正在使用公共基站。
"""
NTRIP client module
based on client from http://github.com/jcmb/NTRIP
"""
import socket
import sys
import datetime
import base64
import time
import errno
import rtcm3
import ssl
from optparse import OptionParser
import serial
version = 0.1
useragent = "NTRIP MAVProxy/%.1f" % version
class NtripError(Exception):
def __init__(self,message,inner_exception=None):
self.message = message
self.inner_exception = inner_exception
self.exception_info = sys.exc_info()
def __str__(self):
return self.message
class NtripClient(object):
def __init__(self,user="",port=2101,caster="",mountpoint="",host=False,lat=46,lon=122,height=1212,ssl=False,V2=False,):
if sys.version_info.major >= 3:
user = bytearray(user,'ascii')
self.user = base64.b64encode(user)
self.port = port
self.caster = caster
self.mountpoint = mountpoint
if not self.mountpoint.startswith("/"):
self.mountpoint = "/" + self.mountpoint
self.setPosition(lat,lon)
self.height = height
self.ssl = ssl
self.host = host
self.V2 = V2
self.socket = None
self.found_header = False
self.sent_header = False
# RTCM3 parser
self.rtcm3 = rtcm3.RTCM3()
self.last_id = None
if self.port == 443:
# force SSL on port 443
self.ssl = True
def setPosition(self,lat,lon):
self.flagN = "N"
self.flagE = "E"
if lon > 180:
lon = (lon-360)*-1
self.flagE = "W"
elif lon < 0 and lon >= -180:
lon = lon*-1
self.flagE = "W"
elif lon < -180:
lon = lon+360
self.flagE = "E"
else:
self.lon = lon
if lat < 0:
lat = lat*-1
self.flagN = "S"
self.lonDeg = int(lon)
self.latDeg = int(lat)
self.lonMin = (lon-self.lonDeg)*60
self.latMin = (lat-self.latDeg)*60
def getMountPointString(self):
userstr = self.user
if sys.version_info.major >= 3:
userstr = str(userstr,'ascii')
mountPointString = "GET %s HTTP/1.0\r\nUser-Agent: %s\r\nAuthorization: Basic %s\r\n" % (
self.mountpoint,useragent,userstr)
if self.host or self.V2:
hostString = "Host: %s:%i\r\n" % (self.caster,self.port)
mountPointString += hostString
if self.V2:
mountPointString += "Ntrip-Version: Ntrip/2.0\r\n"
mountPointString += "\r\n"
return mountPointString
def getGGAString(self):
Now = datetime.datetime.utcNow()
ggaString = "GPGGA,%02d%02d%04.2f,%02d%011.8f,%1s,%03d%011.8f,1,05,0.19,+00400,M,%5.3f," % (
Now.hour,Now.minute,Now.second,self.latDeg,self.latMin,self.flagN,self.lonDeg,self.lonMin,self.flagE,self.height)
checksum = self.calculateCheckSum(ggaString)
return "$%s*%s\r\n" % (ggaString,checksum)
def calculateCheckSum(self,stringtocheck):
xsum_calc = 0
for char in stringtocheck:
xsum_calc = xsum_calc ^ ord(char)
return "%02X" % xsum_calc
def get_ID(self):
'''get ID of last packet'''
return self.last_id
def read(self):
if self.socket is None:
time.sleep(0.1)
self.connect()
return None
if not self.found_header:
if not self.sent_header:
self.sent_header = True
time.sleep(0.1)
mps = self.getMountPointString()
if sys.version_info.major >= 3:
mps = bytearray(mps,'ascii')
try:
self.socket.sendall(mps)
except Exception:
self.socket = None
return None
try:
casterResponse = self.socket.recv(4096)
except ssl.SSLWantReadError:
return None
except IOError as e:
if e.errno == errno.EWOULDBLOCK:
return None
self.socket = None
casterResponse = ''
if sys.version_info.major >= 3:
casterResponse = str(casterResponse,'ascii')
header_lines = casterResponse.split("\r\n")
for line in header_lines:
if line == "":
self.found_header = True
if line.find("SOURCETABLE") != -1:
raise NtripError("Mount point does not exist")
elif line.find("401 Unauthorized") != -1:
raise NtripError("Unauthorized request")
elif line.find("404 Not Found") != -1:
raise NtripError("Mount Point does not exist")
elif line.find(" 200 OK") != -1:
# Request was valid
try:
gga = self.getGGAString()
if sys.version_info.major >= 3:
gga = bytearray(gga,'ascii')
self.socket.sendall(gga)
except Exception:
self.socket = None
return None
return None
# normal data read
while True:
try:
data = self.socket.recv(1)
except ssl.SSLWantReadError:
return None
except IOError as e:
if e.errno == errno.EWOULDBLOCK:
return None
self.socket.close()
self.socket = None
return None
except Exception:
self.socket.close()
self.socket = None
return None
if len(data) == 0:
self.socket.close()
self.socket = None
return None
if self.rtcm3.read(data):
self.last_id = self.rtcm3.get_packet_ID()
return self.rtcm3.get_packet()
def connect(self):
'''connect to NTRIP server'''
self.sent_header = False
self.found_header = False
sock = socket.socket(socket.AF_INET,socket.soCK_STREAM)
if self.ssl:
sock = ssl.wrap_socket(sock)
try:
error_indicator = sock.connect_ex((self.caster,self.port))
except Exception:
return False
if error_indicator == 0:
sock.setblocking(0)
self.socket = sock
self.rtcm3.reset()
return True
return False
def readLoop(self):
while True:
data = self.read()
if data is None:
continue
##ADDED
ser = serial.Serial("/dev/serial0",9600)
ser.write(data)
##
print("got: ",len(data))
if __name__ == '__main__':
usage = "NtripClient.py [options] [caster] [port] mountpoint"
parser = OptionParser(version=version,usage=usage)
parser.add_option("-u","--user",type="string",dest="user",default="IBS",help="The Ntripcaster username. Default: %default")
parser.add_option("-p","--password",dest="password",help="The Ntripcaster password. Default: %default")
parser.add_option("-o","--org",dest="org",help="Use IBSS and the provided organization for the user. Caster and Port are not needed in this case Default: %default")
parser.add_option("-b","--baSEOrg",dest="baSEOrg",help="The org that the base is in. IBSS Only,assumed to be the user org")
parser.add_option("-t","--latitude",type="float",dest="lat",default=50.09,help="Your latitude. Default: %default")
parser.add_option("-g","--longitude",dest="lon",default=8.66,help="Your longitude. Default: %default")
parser.add_option("-e","--height",dest="height",default=1200,help="Your ellipsoid height. Default: %default")
parser.add_option("-s","--ssl",action="store_true",dest="ssl",default=False,help="Use SSL for the connection")
parser.add_option("-H","--host",dest="host",help="Include host header,should be on for IBSS")
parser.add_option("-2","--V2",dest="V2",help="Make a NTRIP V2 Connection")
(options,args) = parser.parse_args()
ntripArgs = {}
ntripArgs['lat'] = options.lat
ntripArgs['lon'] = options.lon
ntripArgs['height'] = options.height
ntripArgs['host'] = options.host
ntripArgs['user'] = "GRSM:GRSM"
ntripArgs['caster'] = "ntrip.nps.gov"
ntripArgs['port'] = 2101
ntripArgs['mountpoint'] = "GRSM_RTCM3x"
n = NtripClient(**ntripArgs)
n.readLoop()
** rtcm3.py
'''Decode RTCM v3 messages'''
RTCMv3_PREAMBLE = 0xD3
polyCRC24 = 0x1864CFB
import struct
class RTCM3:
def __init__(self,debug=False):
self.crc_table = None
self.debug = debug
self.reset()
def get_packet(self):
'''return bytearray of last parsed packet'''
return self.parsed_pkt
def get_packet_ID(self):
'''get get of packet,or None'''
if self.parsed_pkt is None or len(self.parsed_pkt) < 8:
return None
id,= struct.unpack('>H',self.parsed_pkt[3:5])
id >>= 4
return id
def reset(self):
'''reset state'''
self.pkt = bytearray()
self.pkt_len = 0
self.parsed_pkt = None
def parse(self):
'''parse packet'''
parity = self.pkt[-3:]
crc1 = parity[0] << 16 | parity[1] << 8 | parity[2]
crc2 = self.crc24(self.pkt[:-3])
if crc1 != crc2:
if self.debug:
print("crc fail len=%u" % len(self.pkt))
# look for preamble
idx = self.pkt[1:].find(bytearray([RTCMv3_PREAMBLE]))
if idx >= 0:
self.pkt = self.pkt[1+idx:]
if len(self.pkt) >= 3:
self.pkt_len,self.pkt[1:3])
self.pkt_len &= 0x3ff
else:
self.pkt_len = 0
else:
self.reset()
return False
# got a good packet
self.parsed_pkt = self.pkt
self.pkt = bytearray()
self.pkt_len = 0
return True
def read(self,byte):
'''read in one byte,return true if a full packet is available'''
#import random
#if random.uniform(0,1000) < 1:
# return False
byte = ord(byte)
if len(self.pkt) == 0 and byte != RTCMv3_PREAMBLE:
# discard
return False
self.pkt.append(byte)
if self.pkt_len == 0 and len(self.pkt) >= 3:
self.pkt_len,self.pkt[1:3])
self.pkt_len &= 0x3ff
if self.pkt_len == 0:
self.reset()
return False
if self.pkt_len > 0 and len(self.pkt) >= 3 + self.pkt_len + 3:
remainder = self.pkt[6+self.pkt_len:]
self.pkt = self.pkt[:6+self.pkt_len]
# got header,packet body and parity
ret = self.parse()
self.pkt.extend(remainder)
return ret
# need more bytes
return False
def crc24(self,bytes):
'''calculate 24 bit crc'''
if self.crc_table is None:
# initialise table
self.crc_table = [0] * 256
for i in range(256):
self.crc_table[i] = i<<16
for j in range(8):
self.crc_table[i] <<= 1
if (self.crc_table[i] & 0x1000000):
self.crc_table[i] ^= polyCRC24
crc = 0
for b in bytes:
crc = ((crc<<8)&0xFFFFFF) ^ self.crc_table[(crc>>16) ^ b]
return crc
if __name__ == '__main__':
from argparse import ArgumentParser
import time
parser = ArgumentParser(description='RTCM3 parser')
parser.add_argument("filename",type=str,help="input file")
parser.add_argument("--debug",action='store_true',help="show errors")
parser.add_argument("--follow",help="continue reading on EOF")
args = parser.parse_args()
rtcm3 = RTCM3(args.debug)
f = open(args.filename,'rb')
while True:
b = f.read(1)
if len(b) == 0:
if args.follow:
time.sleep(0.1)
continue
break
if rtcm3.read(b):
print("packet len %u ID %u" % (len(rtcm3.get_packet()),rtcm3.get_packet_ID()))
解决方法
检查RPI的串行端口是否已启用。如果不是,则遵循RPI的启用过程。也许试试这个https://learn.adafruit.com/adafruits-raspberry-pi-lesson-5-using-a-console-cable/enabling-serial-console
除此之外,RTCM数据本质上是纯原始数据,因此,您只需要将其原样转发到gps模块(不处理任何内容)即可。如果仍然无法正常工作,那么主要问题是,您是否正在从基站获取正确的RTCM数据。
,我在python上工作不多,因为处理速度稍慢。因此,我用C语言编写了代码,这要快得多。而且,很久以前,我已经基于串行通信创建了一个应用程序,因此,我正在共享一些模块,这可能对您有所帮助。
关于您的查询,您必须在循环中调用函数Serial.write(data),还尝试在两个数据字节延迟(10)之间设置较小的(微秒)延迟。最后,我无法回答您到底需要在应用程序中放置serial.write()的什么地方,您只需要将其放置在接收完整RTCM数据的地方,因此,您可以将其向前推进
建议:转到C语言的lwirinngPi库。对于原始数据处理来说非常容易。
#------------Standard Imports------------
import serial
#------------serial variable--------------
Serial_portName = "/dev/ttyS0"
Serial_BaudRate = 115200
ComPortReciveFlag = False
ComPortRecieveBuff = ""
#----------------------------------------------------------------------
def ReadSerialData():
global ComPortRecieveBuff
global ComPortReciveFlag
if (ser.inWaiting()>0):
ComPortRecieveBuff = ser.read()
sleep(0.03)
data_left = ser.inWaiting()
ComPortRecieveBuff += ser.read(data_left)
ComPortReciveFlag = True
#----------------------------------------------------------------------
def AutoStart_window(self):
global ComPortRecieveBuff,ComPortReciveFlag
#--do process here....
while True:
#--for receiving
#--received_data = ReadSerialData()
#--if (ComPortReciveFlag == True):
#--ComPortReciveFlag = False
#--process on received data
#--for Sending
#--ser.reset_output_buffer()
#--ser.write("# START $".encode()) #incase of string data
#--for unknown data type
#--for x in range(data_length)
#-- ser.write(Txbuffer[x])
#---------Main Code Starting Point--------
#---------Serial ComPort--------
ser = serial.Serial (Serial_portName,Serial_BaudRate)