			#!/usr/bin/env python3

			import sys, datetime
			sys.path.append ('../lib')
			from nmeaTransceiver import *
			from geoCalc import *
			from nmea0183 import RMB, RMC, HDT, MWV
			from router_v1 import *


			class TestBoat:
			"Test boat that receives RMBs and sends RMCs"

			__NT = None
			__RMB = None
			__RMC = None
			__HDT = None
			__MWV = None
			__GC = None

			__nbOfLegs = None

			# Starting position
			__lat = '4301.000'
			__latNS = 'N'
			__lon = '00617.000'
			__lonEW = 'E'

			# Current destination information
			__RMB_lat = None
			__RMB_latNS = None
			__RMB_lon = None
			__RMB_lonEW = None
			__RMB_text = None
			__RMB_iteration = None
			__RMB_waypoints = None

			def __RMB_reader(self, nmeaSentence):
			print ("######### RMB received from EC talker: %s" % nmeaSentence)
			rmbData = self.__RMB.decode(nmeaSentence)

			if rmbData['ArrivalStatus'] == "A" :
			print ("Arrived !!!")
			self.b.wp = None
			return

			# move boat to the next Waypoint
			rmb_text = rmbData['DestinationWaypointLatitude']+rmbData['DestinationWaypointLatitudeNS']+rmbData['DestinationWaypointLongitude']+rmbData['DestinationWaypointLongitudeEW']
			if rmb_text != self.__RMB_text:
			print("New destination detected!")
			# New destination
			self.__RMB_text = rmb_text
			self.__RMB_lat = rmbData['DestinationWaypointLatitude']
			self.__RMB_latNS = rmbData['DestinationWaypointLatitudeNS']
			self.__RMB_lon = rmbData['DestinationWaypointLongitude']
			self.__RMB_lonEW = rmbData['DestinationWaypointLongitudeEW']
			self.__RMB_iteration = 0

			lat = Lat_Nmea2Dec(self.__RMB_lat, self.__RMB_latNS)
			lon = Lon_Nmea2Dec(self.__RMB_lon, self.__RMB_lonEW)
			self.b.wp = WayPoint(lat, lon)
			self.b.prevTak = 0
			print ('WP {0:.3f} {1:.3f}'.format(lat,lon))
			Lat_Dec2Nmea(lat)
			Lon_Dec2Nmea(lon)

			'''
			# Compute intermediate waypoints
			currentPosition = NmeaGeoPoint(self.__lat, self.__latNS, self.__lon, self.__lonEW)
			destinationPosition = NmeaGeoPoint( rmbData['DestinationWaypointLatitude'],
			rmbData['DestinationWaypointLatitudeNS'],
			rmbData['DestinationWaypointLongitude'],
			rmbData['DestinationWaypointLongitudeEW'])
			self.__RMB_waypoints = self.__GC.getIntermediateWaypoints(currentPosition, destinationPosition, self.__nbOfLegs)['waypoints']
			'''


			def __RMC_sender(self):

			# Move to the next intermediate waypoint
			'''
			bearing = 0
			if self.__RMB_iteration != None:
			n = self.__RMB_iteration
			if n < self.__nbOfLegs:
			print("Moving to waypoint: #",n+1,"/",len(self.__RMB_waypoints))
			targetPoint = GeoPoint(self.__RMB_waypoints[n]['latitude'], self.__RMB_waypoints[n]['longitude'])
			print(str(targetPoint))
			bearing = self.__RMB_waypoints[n]['bearing']
			self.__lat, self.__latNS, self.__lon, self.__lonEW = targetPoint.toNMEA()
			self.__RMB_iteration += 1
			else:
			self.__RMB_iteration = None
			'''
			if self.b.wp == None :
			return ''
			self.b.iter()
			print('Iteration #{0:3d} Time : {1:3d} sec'.format(self.b._iter, self.b._iter*self.b.Time))
			print(' WP Bearing:{0:.3f} Dist:{1:.3f} Nm'.format(self.b.azimut, self.b.dist))
			print(' TWA:{0:.3f}'.format(self.b.twa))
			print(' TWS:{0:.3f}'.format(self.b.wind.speed))
			print(' boat speed:{0:.3f} Kts'.format(self.b.speed))
			print(' VMG:{0:.3f} Kts'.format(self.b.vmg))
			if self.b.vmg != 0 :
			print(' ETA:{0:.3f} sec = {1:.3f} h'.format(3600*self.b.dist/self.b.vmg, self.b.dist/self.b.vmg))
			print(' Heading:{0:.3f}'.format(self.b.hdg))
			print(' New position : {0:.5f} {1:.5f}'.format(self.b.lat, self.b.lon))

			now = datetime.datetime.now()
			nmea = self.__RMC.encodeDict( { 'UtcOfPositionFix': now.strftime('%H%M%S'),
			'Status': 'A',
			'Latitude': Lat_Dec2Nmea(self.b.lat),
			'LatitudeNS': self.__latNS,
			'Longitude': Lon_Dec2Nmea(self.b.lon),
			'LongitudeEW': self.__lonEW,
			'SpeedOverGround': str(self.b.speed),
			'CourseOverGround': str(self.b.hdg),
			'Date': now.strftime('%d%m%y'),
			'MagneticVariation': '0.0',
			'ModeIndicator': 'E'
			} )
			print (nmea)
			return nmea

			def __HDT_sender(self):
			nmea = self.__HDT.encodeDict( { 'Heading': str(self.b.hdg) } )
			print (nmea)
			return nmea

			def __MWV_sender(self):
			nmea = self.__MWV.encodeDict( { 'Angle': '30',
			'Reference': 'R',
			'Speed': '15.5',
			'SpeedUnits': 'N'
			} )
			print (nmea)
			return nmea

			def __init__(self, nbOfLegs = 1):
			self.b = Boat(42.9, 6.2)
			self.b.Time = 60
			self.b.polar = Polar('../polar/so36i.pol')
			self.b.wind = Wind(0.0, 15.0)
			#self.b.wp = WayPoint(43.0, 6.0) # Start at least with a WP
			# Set default parameters
			if nbOfLegs < 1:
			self.__nbOfLegs = 1
			else:
			self.__nbOfLegs = nbOfLegs


			# Create NMEA messages handlers
			self.__RMB = RMB(RMB.nmeaAttributes)
			self.__RMC = RMC(RMC.nmeaAttributes, 'XX')
			self.__HDT = HDT(HDT.nmeaAttributes, 'XX')
			self.__MWV = MWV(MWV.nmeaAttributes, 'XX')

			# Create transceiver
			self.__NT = NmeaTransceiver(sys.argv)

			# Create GeoCal
			self.__GC = GeoCalc()

			# Add a read callback, called when a RMB is received (only from EC talker)
			self.__NT.addNmeaFilteringReader(self.__RMB_reader, 'RMB', 'EC')

			# Call RMC sender ASAP (when transceiver is started) and repeat every 3 seconds
			self.__NT.callSoon(self.__RMC_sender, 1)
			self.__NT.callSoon(self.__HDT_sender, 1)
			self.__NT.callSoon(self.__MWV_sender, 1)

			def start(self):
			self.__NT.startTransceiver()

			#
			# Main
			#


			TB = TestBoat(60)
			TB.start()

			# EOF

mux0183/tools/sailboat
11ea4bcb6a98master

sailboat
View Options

mux0183/tools/sailboat11ea4bcb6a98master

sailboatView Options

mux0183/tools/sailboat
11ea4bcb6a98master

sailboat
View Options