diff --git a/lib/nmea0183.py b/lib/nmea0183.py index a3161b9..6c069e2 100755 --- a/lib/nmea0183.py +++ b/lib/nmea0183.py @@ -1,165 +1,246 @@ #!/usr/bin/env python3 import re from collections import OrderedDict # USAGE: # # 1) create instance, add data with fromList() or fromDict(), [add more fields,] then encode() # 2) create instance, add data with fromString(), [set filter with applyFilter(),] then decode() # # Once the decode() or encode() methods have been called, the prettyPrint() method can be called. class NMEA0183: "NMEA0183 encoder/decoder" __checksumSeparator = '*' __fieldSeparator = ',' __EOL = '\r\n' + __nmeaOrder = None def __init__(self, sentenceStart = '$'): self.__sentenceStart = sentenceStart - def fromList(self, talkerId, msgType, msgDict = []): + def fromList(self, talkerId, msgType, msgList = []): self.sync = False self.talkerId = talkerId self.msgType = msgType self.msgDict = OrderedDict() - for item in msgDict: - self.addUnamedField(item) - - def fromDict(self, talkerId, msgType, msgDict = ()): + if self.__nmeaOrder != None: + for i, key in enumerate(self.__nmeaOrder): + try: + value = msgList[i] + except IndexError: + value = None + self.addField(key, value) + else: + for item in msgList: + self.addUnamedField(item) + + def setNmeaOrder(self, nmeaOrder = None): + self.__nmeaOrder = nmeaOrder + + def fromDict(self, talkerId, msgType, msgDict = {}): self.sync = False self.talkerId = talkerId self.msgType = msgType - self.msgDict = OrderedDict(msgDict) + if self.__nmeaOrder != None: + # NMEA attributes for this message type are known + self.msgDict = OrderedDict() + for key in self.__nmeaOrder: + self.addField(key, msgDict.get(key, None)) + else: + # Use the provided Dict as is + self.msgDict = OrderedDict(msgDict) def addField(self, key, value): self.msgDict[key] = value def addUnamedField(self, value): self.msgDict['field_'+str(len(self.msgDict))] = value def encode(self): self.msgString = self.__sentenceStart + self.talkerId + self.msgType - for key in self.msgDict: - self.msgString += NMEA0183.__fieldSeparator + self.msgDict[key] + for i, key in enumerate(self.msgDict): + value = self.msgDict[key] + if value == None: + value = '' + if value != '' and i < len(self.msgDict): + self.msgString += NMEA0183.__fieldSeparator + self.msgDict[key] self.msgString += NMEA0183.__checksumSeparator + self.__calculateChecksum(self.msgString) self.msgString += NMEA0183.__EOL self.sync = True return self.msgString def fromString(self, msgString): self.sync = False self.msgDict = OrderedDict() self.msgString = msgString.rstrip() self.talkerId = self.msgString[1:3] self.msgType = self.msgString[3:6] self.msgContent = self.msgString[7:-3] self.isValid = self.__validate() return self.isValid def applyFilter(self, msgTalkerIdFilter = '', msgTypeFilter = ''): if not self.isValid: return False if msgTalkerIdFilter != '': pattern = re.compile(msgTalkerIdFilter) match = pattern.match(self.talkerId) if msgTypeFilter == '' and not match: return False if msgTypeFilter != '': pattern = re.compile(msgTypeFilter) return bool(pattern.match(self.msgType)) return True def decode(self): if not self.isValid: return False - i=0 - for value in self.msgContent.split(NMEA0183.__fieldSeparator): - self.msgDict['field_'+str(i)] = value - i += 1 + if self.__nmeaOrder != None: + values = self.msgContent.split(NMEA0183.__fieldSeparator) + for i, key in enumerate(self.__nmeaOrder): + try: + self.msgDict[self.__nmeaOrder[i]] = values[i] + except IndexError: + self.msgDict[self.__nmeaOrder[i]] = None + else: + i=0 + for value in self.msgContent.split(NMEA0183.__fieldSeparator): + self.msgDict['field_'+str(i)] = value + i += 1 self.sync = True return self.msgDict - def __validate(self): self.msgChecksumSeprator = self.msgString[-3:-2] if self.msgChecksumSeprator != NMEA0183.__checksumSeparator: return False self.msgChecksum = self.msgString[-2:] return self.msgChecksum == self.__calculateChecksum(self.msgString[0:-3]) def __calculateChecksum(self, nmeaString): checksum = 0 for c in nmeaString[1:]: checksum ^= ord(c) return format(checksum, '02X') def prettyPrint(self): if self.sync: print('NMEA sentence: '+self.msgString.rstrip()) i = 0 for item in self.msgDict.items(): print("%2.2d %s" % (i, str(item))) i += 1 print('---') -# Implement DPT message -class DPT(NMEA0183): - def __init__(self): - super().__init__('$') - - def encode(self, depth, offset): - super().fromDict('YX','DPT') - super().addField('Depth', depth) - super().addField('Offset', offset) +# +# Implement a Standard Message +# + +class StdMessage(NMEA0183): + __talkerId = None + __msgType = None + + def __init__(self, nmeaOrder = [ 'field_0' ], talkerId = '??', sentenceStart = '$'): + super().__init__(sentenceStart) + self.__talkerId = talkerId + self.__msgType = type(self).__name__ + self.setNmeaOrder(nmeaOrder) + + def encodeList(self, dataList): + super().fromList(self.__talkerId, self.__msgType, dataList) return(super().encode()) + def encodeDict(self, dataDict): + super().fromDict(self.__talkerId, self.__msgType, dataDict) + return(super().encode()) + + def decode(self, msgString): super().fromString(msgString) return super().decode() + +# Implement DPT message +class DPT(StdMessage): + nmeaAttributes = ['Depth', 'Offset'] + + +# Implement RMB message +class RMB(StdMessage): + nmeaAttributes = [ 'DataStatus', + 'CrossTrackError', + 'DirectionToSteer', + 'OriginWaypointId', + 'DestinationWaypointId', + 'DestinationWaypointLatitude', + 'DestinationWaypointLatitudeNS', + 'DestinationWaypointLongitude', + 'DestinationWaypointLongitudeEW', + 'RangeToDestination', + 'BearingToDestination', + 'ArrivalStatus', + 'ModeIndicator' ] + +# Implement RMB message +class RMC(StdMessage): + nmeaAttributes = [ 'UtcOfPositionFix', + 'Status', + 'Latitude', + 'LatitudeNS', + 'Longitude', + 'LongitudeEW', + 'SpeedOverGround', + 'CourseOverGround', + 'Date', + 'MagneticVariation', + 'ModeIndicator' ] + + # # Code for testing classes # if __name__ == '__main__': import sys #sys.path.append ('../lib') #from nmea0183 import * msg = NMEA0183() # Build GGA sentence data = ['184353.07', '1929.045', 'S', '02410.506', 'E', '1', '04', '2.6', '100.00', 'M', '-33.9', 'M'] msg.fromList('GP', 'GGA', data) msg.addField('toto', '') msg.addUnamedField('0000') txt = msg.encode() msg.prettyPrint() # Build GGA sentence data = ((1,'184353.07'),(2,'1929.045'),(3,'S'),(4,'02410.506'),(5,'E'),(6,'1'),(7,'04'),(8,'2.6'),(9,'100.00'),(10,'M'),(11,'-33.9'),(12,'M')) msg.fromDict('GP', 'GGA', data) msg.addField('toto', '') msg.addUnamedField('0000') txt = msg.encode() msg.prettyPrint() # Read GGA sentence with filter msg.fromString(txt) if msg.applyFilter('GP', '(POV|.GA)'): msg.decode() msg.prettyPrint() else: print('Filtered.') - # Build DPT and decode it - msg2 = DPT() - txt = msg2.encode('3.3','0.7') + # Build several DPTs and decode them + msg2 = DPT(DPT.nmeaAttributes, 'XY') + txt = msg2.encodeList(['3.3']) + msg2.prettyPrint() + txt = msg2.encodeDict( { 'toto': '1', 'Depth': '4.5', 'Offset': '0.7'} ) msg2.prettyPrint() msg2.decode(txt) msg2.prettyPrint() # EOF diff --git a/lib/nmeaSender.py b/lib/nmeaSender.py index 1f1f29f..311bf78 100755 --- a/lib/nmeaSender.py +++ b/lib/nmeaSender.py @@ -1,75 +1,75 @@ #!/usr/bin/env python3 # # Fake depth gauge (sending DPT) # import socket, sys class NmeaSender: - "NMEA0183 sender. Connects to a server an sends sentenses" + "NMEA0183 sender. Connects to a server and sends sentenses" __port = None __host = None __s = None RECV_BUFFER = 4096 # Advisable to keep it as an exponent of 2 def __init__(self, argv): if(len(argv) < 2): print("Usage : " + argv[0] + " port [IP]") sys.exit() self.__port = int(argv[1]) if len(argv) == 3: self.__host = argv[2] else: self.__host = "0.0.0.0" self.__connect() def __connect(self): print("Connecting to server (" + self.__host + ":" + str(self.__port) + ")") try: - self.s = socket.socket() - self.s.connect((self.__host,self.__port)) + self.__s = socket.socket() + self.__s.connect((self.__host,self.__port)) except: print("Error while connecting.") sys.exit() print("Connected. Press Ctrl-C to quit.") def send(self, nmeaString): try: - self.s.send(bytes(nmeaString, 'ASCII')) + self.__s.send(bytes(nmeaString, 'ASCII')) except Exception: print print("Error while sending NMEA message: %" % Exception) self.terminate() def close(self): print("Disconnected.") - self.s.close() + self.__s.close() def terminate(self): - self.s.close() + self.__s.close() print("Done.") sys.exit() # # Code for testing classes # if __name__ == '__main__': import time, random #sys.path.append ('../lib') #from nmeaSender import * from nmea0183 import DPT nS = NmeaSender(sys.argv) while 1: nS.send(DPT().encode(str(random.randint(5,10)), '0.7')) try: time.sleep(1) except: nS.terminate() # EOF diff --git a/tools/boat b/tools/boat index 0a2c8ff..995f0c4 100755 --- a/tools/boat +++ b/tools/boat @@ -1,99 +1,71 @@ #!/usr/bin/env python3 -# -# Boat simulator -# -# Send RMC position each second -# When receiving RMB from OpenCPN, jump to the WP -# +import sys, datetime +sys.path.append ('../lib') +from nmeaTransceiver import * +from nmea0183 import RMB, RMC -import threading, socket, time, sys, pynmea2, datetime +class TestBoat: + "Test boat that receives RMBs and sends RMCs..." -# Define start position -lat='4301.000' -latNS='N' -lon='00617.000' -lonEW='E' + __NT = None + __RMB = None + __RMC = None -def boatThread(nb, nom = ''): - prev_lat = '0' - prev_lon = '0' - while 1: - global lat, lon, quit - if (prev_lat != lat) or (prev_lon != lon) : - quit = 0; - else : - quit += 1 - if quit > 20 : - print("Timeout reach: no new RMB received.") - client.close() - sys.exit() - # $GPRMC,165905.56,A,4228.634,N,00610.659,E,6.0,183.0,210816,-nan,E,*1B - msg = pynmea2.RMC( 'GP', 'RMC', ( - datetime.datetime.now().strftime('%H%M%S'), # TIMESTAMP hhmmss - 'A', - lat, - latNS, - lon, - lonEW, - '5.9', # SOG - '183.0', # Bearing - datetime.datetime.now().strftime('%d%m%y'), # DATE ddmmyy - '0.0', # Magnetic variation - 'E' - )) - try: - client.send(bytes(str(msg),'ASCII')) - except OSError as errormsg: - print("Error while sending RMC: %s" % errormsg) - sys.exit() + __lat = '4301.000' + __latNS = 'N' + __lon = '00617.000' + __lonEW = 'E' - # Sleep 1 second - time.sleep(1) - prev_lat = lat - prev_lon = lon - -# $ECRMB,A,0.214,L,001,002,4301.550,N,00617.937,E,0.880,51.213,6.000,V*2A -def fromOpenCPN(sentence): - global lat, lon - try : - msg = pynmea2.parse(sentence) - if msg.sentence_type == 'RMB': - print("Parsed: %s" % msg) - lat = msg.data[5] - latNS = msg.data[6] - lon = msg.data[7] - lonEW = msg.data[8] - print('Changed to '+lat+' '+lon) - else: - print("Ignored: %s" % msg.sentence_type) - except : - print('???') + def __reader(self, nmeaSentence): + print("Boat received (after filtering): %s" % nmeaSentence) + rmbData = self.__RMB.decode(nmeaSentence) + + # move boat to the next Waypoint + self.__lat = rmbData['DestinationWaypointLatitude'] + self.__latNS = rmbData['DestinationWaypointLatitudeNS'] + self.__lon = rmbData['DestinationWaypointLongitude'] + self.__lonEW = rmbData['DestinationWaypointLongitudeEW'] + #self.__RMB.prettyPrint() + + def __writer(self): + sentence = self.__RMC.encodeDict( { 'UtcOfPositionFix': datetime.datetime.now().strftime('%H%M%S'), + 'Status': 'A', + 'Latitude': self.__lat, + 'LatitudeNS': self.__latNS, + 'Longitude': self.__lon, + 'LongitudeEW': self.__lonEW, + 'SpeedOverGround': '4.6', + 'CourseOverGround': '198', + 'Date': datetime.datetime.now().strftime('%d%m%y'), + 'MagneticVariation': '0.0', + 'ModeIndicator': 'E' + } ) + return sentence -###### main ##### + def __init__(self): + # Messages handlers + self.__RMB = RMB(RMB.nmeaAttributes) + self.__RMC = RMC(RMC.nmeaAttributes, 'GP') -# Connect to multiplexer -client = socket.socket() -host = socket.gethostname() -client.connect(('0.0.0.0', 39150)) + # Transceiver + self.__NT = NmeaTransceiver(sys.argv) -# Starts boat thread (for $GPRMC) -a = threading.Thread(None, boatThread, None, (200,), {'nom':'thread a'}) -a.start() + # Read callback, called when a RMB is received + self.__NT.addNmeaFilteringReader(self.__reader, 'EC', 'RMB') + + # Write callback, called every 3 seconds + self.__NT.callSoon(self.__writer, 3) + + def startBoat(self): + self.__NT.startTransceiver() + +# +# Main +# -while 1: - try: - resp = client.recv(255).decode() - except: - print - print("Killing boat...") - global quit - quit = 1000; - sys.exit(); +TB = TestBoat() +TB.startBoat() - if not resp: - break - sentences = resp.splitlines() - for sentence in sentences : - fromOpenCPN(sentence) +# EOF