Homec4science
Diffusion mux0183 (master) Browse

mux0183/tools/rboat
ec43366ef8edmaster

Last Change
Raw File

rboat
View Options

#!/usr/bin/env python3
import sys, datetime
sys.path.append ('../lib')
from nmeaTransceiver import *
from geoCalc import *
from nmea0183 import RMB, RMC
from router_v1 import *
def Lat_Nmea2Dec(nmea, NS) :
_int = int(nmea[0:2])
_dec = float(nmea[2:])
lat = _int + (_dec/60)
if NS == 'S' :
lat = -lat
#print ('{0} {1} {2}'.format(_int, _dec, lat))
return lat
def Lon_Nmea2Dec(nmea, EW) :
_int = int(nmea[0:3])
_dec = float(nmea[3:])
lon = _int + (_dec/60)
if EW == 'W' :
lon = -lon
#print ('{0} {1} {2}'.format(_int, _dec, lon))
return lon
def Lat_Dec2Nmea(lat) :
_int = int(lat)
_dec = (lat - _int) * 60
nmea = '{0:02d}{1:06.3f}'.format(_int, _dec)
#print ('{0} {1} {2}'.format(_int, _dec, nmea))
return nmea
def Lon_Dec2Nmea(lon) :
_int = int(lon)
_dec = (lon - _int) * 60
nmea = '{0:03d}{1:06.3f}'.format(_int, _dec)
#print ('{0} {1} {2}'.format(_int, _dec, nmea))
return nmea
class TestBoat:
"Test boat that receives RMBs and sends RMCs"
__NT = None
__RMB = None
__RMC = 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)
# 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 = WP(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 Azimut:{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 __init__(self, nbOfLegs = 1):
self.b = Boat(42.9, 6.2, 90.0, 0)
self.b.Time = 60
self.b.polar = Polar('../lib/boat_figaro2.csv')
self.b.wind = Wind(90.0, 15.0)
# 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, 'GP')
# 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)
def start(self):
self.__NT.startTransceiver()
#
# Main
#
TB = TestBoat(60)
TB.start()
# EOF
c4science · Help