I have a GPS RTK2 - Zed F9P board hooked to a raspberry pi with the following connections.
Rpi TX → Rtk RX2
Rpi RX → Rtk TX2
Ground - Ground
5v - 5v
On the raspberry pi I am running the following code to connect to an NTRIP base station and then try to send the RTCM3 data into the GPS RTK2 board through the rpi serial port. However, when I run the code, and I check U-Center, the GPS is not going into RTK mode. Do I have the Serial.Write in the correct place? Not sure what is going on here. I am running ntrip.py (ntrip.py needs rtcm3.py as an import)
ntrip.py
"""
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,%1s,1,05,0.19,+00400,M,%5.3f,M,," % (
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", 115200)
ser.write(data)
time.sleep(0.01)
#
#print(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", type="string", dest="password", default="IBS", help="The Ntripcaster password. Default: %default")
parser.add_option("-o", "--org", type="string", 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", type="string", 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", type="float", dest="lon", default=8.66, help="Your longitude. Default: %default")
parser.add_option("-e", "--height", type="float", 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", action="store_true", dest="host", default=False, help="Include host header, should be on for IBSS")
parser.add_option("-2", "--V2", action="store_true", dest="V2", default=False, 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
##ADDED
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, = struct.unpack('>H', 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, = struct.unpack('>H', 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", action='store_true', 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()))