Homec4science
Diffusion mux0183 (master) Browse

mux0183/lib/router_v1.py
b2e950483598master

Last Change
Raw File

router_v1.py
View Options

#!/usr/bin/env python3
import math, time
from geographiclib.geodesic import Geodesic
from polar2 import Polar
"""
--------------------
This is a very simple router algorithm
- very simple polar
- don't handle derive
- don't handle streams
- don't converge quickly
--------------------
"""
class Wind :
def __init__(self, dir, speed) :
self.dir = dir
self.speed = speed
class Boat :
def __init__(self, lat, lon, hdg, speed) :
# constants
self.Time = 10 # iteration time, in seconds
self.Precision = 5 # best vmg calculation, in degrees
self.GotoDirect = 0.20 # minimum distance to WP to go directly, in Nm
self.Circle = 0.01 # arrival circle, in Nm
self.Hysteresis = 600 # minimum time between 2 taks, in secondes
# variables
self.lat = lat
self.lon = lon
self.hdg = hdg
self.speed = speed
self.twa = 0
self.vmg = 0
self.wp = None
self.wind = None
self.polar = None
self._iter = 0
self.wp_reached = False
self.prevTak = 0
self.azimut = 0.0
self.dist = 0.0
# calculate current boat position
def currentPosition(self) :
step = self.Time*(self.speed*1852)/3600
geo = Geodesic.WGS84.Direct(self.lat, self.lon, self.hdg, step)
self.lat = geo['lat2']
self.lon = geo['lon2']
# calculate heading and distance from WP
def distanceToWP(self) :
geo = Geodesic.WGS84.Inverse(self.lat, self.lon, self.wp.lat, self.wp.lon)
self.dist = geo['s12']/1852
self.azimut = geo['azi1']
def Vmg(self, hdg) :
twa = angleDiff(hdg, self.wind.dir)
#print ('hdg:{0} wind:{1} twa:{2}'.format(hdg,self.wind.dir,twa))
twa = math.fabs(twa)
x = angleDiff(hdg, self.azimut)
x = math.fabs(x)
speed = self.polar.boatSpeed(twa, self.wind.speed)
vmg = speed * cosd(x)
return vmg,twa,speed
def _bestVmg(self) :
best_vmg = 0
best_twa = 0
best_hdg = 0
best_speed = 0
hdg = self.hdg
while True :
vmg,twa,speed = self.Vmg(hdg)
if twa > 180 :
break
if vmg > best_vmg :
best_vmg = vmg
best_twa = twa
best_hdg = hdg
best_speed = speed
hdg += self.Precision
if hdg == 360 :
hdg = 0
if hdg == self.hdg :
break;
if best_hdg > 360 :
best_hdg -= 360
return best_vmg, best_twa, best_hdg, best_speed
def bestVmg(self) :
self.distanceToWP()
return self._bestVmg()
def iter(self) :
if self.wp == None :
return
self.currentPosition()
self.distanceToWP()
# WP reached !
if self.dist <= self.Circle :
self.wp_reached = True
return
vmg, twa, hdg, speed = self._bestVmg()
now = self.Time * self._iter
if (self.prevTak == 0) or (now - self.prevTak >= self.Hysteresis) :
self.vmg = vmg
self.twa = twa
self.hdg = hdg
self.speed = speed
self.prevTak = now
# Near WP => direct goto
if self.dist <= self.GotoDirect :
#print ('*** go direct to WP')
self.hdg = self.azimut
while True :
self.vmg,self.twa,self.speed = self.Vmg(self.hdg)
if self.speed > 0 :
break;
self.hdg += 5
self._iter += 1
class WayPoint :
def __init__(self, lat, lon) :
self.lat = lat
self.lon = lon
def angleDiff(a1, a2) :
d = a1 - a2
if d > 180 :
d -= 360
elif d < -180 :
d += 360
#print('diff '+str(d))
return d
# sinus degrees
def sind(deg) :
return math.sin(math.radians(deg))
# cosinus degrees
def cosd(deg) :
return math.cos(math.radians(deg))
#--------------------
# Code for testing classes
#--------------------
if __name__ == '__main__':
b = Boat(43.0, 6.0, 90.0, 5.0)
b.polar = Polar('../polar/so36i.pol')
b.wind = Wind(90.0, 15.0)
b.wp = WayPoint(43.1, 6.1)
while b.wp_reached == False :
print('Iteration #{0:3d} Time : {1:3d} sec'.format(b._iter, b._iter*b.Time))
print(' WP Azimut:{0:.3f} Dist:{1:.3f} Nm'.format(b.azimut, b.dist))
b.iter()
print(' Heading:{0:.3f}'.format(b.hdg))
print(' TWA:{0:.3f}'.format(b.twa))
print(' TWS:{0:.3f}'.format(b.wind.speed))
print(' boat speed:{0:.3f} Kts'.format(b.speed))
print(' VMG:{0:.3f} Kts'.format(b.vmg))
print(' ETA:{0:.3f} sec = {1:.3f} h'.format(3600*b.dist/b.vmg, b.dist/b.vmg))
print(' New position : {0:.5f} {1:.5f}'.format(b.lat, b.lon))
c4science · Help