diff --git a/lib/geoCalc.py b/lib/geoCalc.py
index 5603596..8de8039 100755
--- a/lib/geoCalc.py
+++ b/lib/geoCalc.py
@@ -1,65 +1,171 @@
 #!/usr/bin/env python3
 
-class GeoConversion:
-    ""
-
-    def __init__():
-        pass
-
-    def dms2dd(self, d, m, s):
-        dd = float(degrees) + float(minutes)/60 + float(seconds)/(3600)
-        return dd
+from geographiclib.geodesic import Geodesic
+import re, math
+
+class GeoPoint:
+
+    latitude  = None
+    longitude = None
+    name = None
+
+    def __dmsh2dd(self, dmsh):
+        pattern='^[0-9]+°([0-5]?[0-9]\'([0-5]?[0-9]\")?)?[NSEW]$'
+        if re.match(pattern, dmsh):
+            h = dmsh[-1:]
+            dms = dmsh[0:-1]
+            pattern='[\°\'\"]'
+            sp = re.split(pattern, dms)
+            dd = 0
+            for i, value in enumerate(sp):
+                if value != '':
+                    dd += float(value)/(60**i)
+            if h == 'S' or h == 'W':
+                dd *= -1
+            return dd
+        return None
+
+    def __ddh2dd(self, ddh):
+        pattern='^[0-9]+(\.[0-9]+)?[NSEW]$'
+        if re.match(pattern, ddh):
+            h = ddh[-1:]
+            dd = float(ddh[0:-1])
+            if h == 'S' or h == 'W':
+                dd *= -1
+            return dd
+        return None
+ 
+
+    def __init__(self, lat = None, lon = None, name = None):
+        if name != None:
+            self.name = name
+        # Latitude
+        lat_done = False
+        if isinstance(lat, float) or isinstance(lat, int):
+            self.latitude = float(lat)
+            lat_done = True
+        if not lat_done and isinstance(lat, str):
+            self.latitude = self.__ddh2dd(lat)
+            if self.latitude != None:
+                lat_done = True
+        if not lat_done and isinstance(lat, str):
+            self.latitude = self.__dmsh2dd(lat)
+            if self.latitude != None:
+                lat_done = True
+        if not lat_done:
+            raise ValueError("Unknown format provided (latitude): "+str(lat))
+        # Longitude
+        lon_done = False
+        if isinstance(lon, float) or isinstance(lon, int):
+            self.longitude = float(lon)
+            lon_done = True
+        if not lon_done and isinstance(lon, str):
+            self.longitude = self.__ddh2dd(lon)
+            if self.longitude != None:
+                lon_done = True
+        if not lon_done and isinstance(lon, str):
+            self.longitude = self.__dmsh2dd(lon)
+            if self.longitude != None:
+                lon_done = True
+        if not lon_done:
+            raise ValueError("Unknown format provided (longitude): "+str(lon))
+
+    def isValid(self):
+        return bool(self.longitude != None)
+
+    def toNMEA(self):
+        lat = str(abs(self.latitude*100))
+        if self.latitude < 0:
+            latNS = 'S'
+        else:
+            latNS = 'N'
+        lon = str(abs(self.longitude*100))
+        if self.longitude < 0:
+            lonEW = 'W'
+        else:
+            lonEW = 'E'
+        return (lat, latNS, lon, lonEW)
+
+    def __str__(self):
+        txt = ''
+        if self.name != None:
+            txt = self.name + ': '
+        if self.isValid():
+            txt += '('+str(self.latitude)+', '+str(self.longitude)+')'
+        else:
+            txt += 'INVALID'
+        return txt
+           
+    def prettyPrint(self):
+        print(str(self))
+
+class NmeaGeoPoint(GeoPoint):
+    def __init__(self, lat, latNS, lon, lonEW):
+        super().__init__(str(float(lat)/100)+latNS, str(float(lon)/100)+lonEW)
 
-    def dd2geopoint(self, dd, h):
-        if h == 'E' or h == 'N':
-            dd *= -1
-        return dd
 
-class NmeaGeoPoint:
+class GeoCalc:
     ""
-    __lat = None
-    __latNS = None
-    __lon = None
-    __geoLat = None
-    __geoLon = None
 
-    def __init__(self, lat, latNS, lon, lonEW):
-        self.__lat = lat
-        self.__latNS = latNS
-        self.__lon = lon
-        self.__lonEW = lonEW
-    
-    def getGeoPoint(self):
-        self.__geoLat = GeoConversion.dd2geopoint(self.__lat, self.__latNS)
-        self.__geoLon = GeoConversion.dd2geopoint(self.__lon, self.__lonEW)
-        return (self.__geoLat, self.__geoLon)
+    __geodesic = None
+
+    def __init__(self):
+        self.__geodesic = Geodesic.WGS84
+
+
+    def initialBearingAndDistance(self, fromPoint, toPoint, prettyPrint = False):
+        g = self.__geodesic.Inverse(fromPoint.latitude, fromPoint.longitude, toPoint.latitude, toPoint.longitude)
+        rv = {}
+        rv['initial_bearing'] = g['azi1']
+        rv['final_bearing'] = g['azi2']
+        rv['distance'] = g['s12']
+        rv['distance_nm'] = g['s12']/1852
+        if bool(prettyPrint):
+            print("FROM:  "+str(fromPoint))
+            print("TO:    "+str(toPoint))
+            print("CALCS: "+str(rv))
+        return rv
+
+    def getIntermediateWaypoints(self, fromPoint, toPoint, nbPoints, prettyPrint = False):
+        inverseLine = self.__geodesic.InverseLine(fromPoint.latitude, fromPoint.longitude, toPoint.latitude, toPoint.longitude)
+        distance = inverseLine.s13
+        rv = {}
+        rv['distance'] = distance
+        rv['distance_nm'] = distance/1852
+
+        # Calculate the waypoints
+        if not isinstance(nbPoints, int):
+            nbPoints = 1
+        waypoints = []
+        ds = distance/nbPoints
+        for i in range(1, nbPoints+1, 1):
+            # Distance from start
+            s = min(ds * i, distance)
+            # Get waypoint
+            g = inverseLine.Position(s, Geodesic.STANDARD | Geodesic.LONG_UNROLL)
+
+            gp = {'id': i,'latitude': g['lat2'], 'longitude': g['lon2'], 'distance_nm': s/1852, 'distance': s, 'bearing': g['azi2']}
+            waypoints.append(gp)
+
+        rv['waypoints'] = waypoints
+        if bool(prettyPrint):
+            print(rv)
         
+        return rv
 
-class GeoCalc:
-    "Class to do calculations with geo coordinates"
-
-    def getInitialBearing(pointA, pointB):
-        "Latitude and longitude must be in decimal degrees"
-        if (type(pointA) != tuple) or (type(pointB) != tuple):
-            raise TypeError("Only tuples are supported as arguments")
-
-        lat1 = math.radians(pointA[0])
-        lat2 = math.radians(pointB[0])
-
-        diffLong = math.radians(pointB[1] - pointA[1])
-
-        x = math.sin(diffLong) * math.cos(lat2)
-        y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) * math.cos(lat2) * math.cos(diffLong))
-
-        initial_bearing = math.atan2(x, y)
-        initial_bearing = math.degrees(initial_bearing)
-
-        # Now we have the initial bearing but math.atan2 return values
-        # from -180° to + 180° which is not what we want for a compass bearing
-        compass_bearing = (initial_bearing + 360) % 360
-        return compass_bearing
-
+# 
+# Code for testing classes
+#
 
+if __name__ == '__main__':
 
+    try:
+        # Create points
+        PointA = GeoPoint(-41.32, 174.81, 'Wellingtoni, NZ')
+        PointB = GeoPoint("40.96N", "5.50W", 'Salamanca, Spain')
+        GC = GeoCalc()
+        GC.initialBearingAndDistance(PointA, PointB, True)
+        GC.getIntermediateWaypoints(PointA, PointB, 5, True)
 
-# EOF
+    except ValueError as error:
+        print(error)
diff --git a/tools/boat b/tools/boat
index bf72def..b8a4749 100755
--- a/tools/boat
+++ b/tools/boat
@@ -1,68 +1,103 @@
 #!/usr/bin/env python3
 
 import sys, datetime
 sys.path.append ('../lib')
 from nmeaTransceiver import *
+from geoCalc import *
 from nmea0183 import RMB, RMC
 
 class TestBoat:
     "Test boat that receives RMBs and sends RMCs"
 
     __NT = None
     __RMB = None
     __RMC = None
+    __GC = None
 
     __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
-        self.__lat = rmbData['DestinationWaypointLatitude']
-        self.__latNS = rmbData['DestinationWaypointLatitudeNS']
-        self.__lon = rmbData['DestinationWaypointLongitude']
-        self.__lonEW = rmbData['DestinationWaypointLongitudeEW']
+        rmb_text = rmbData['DestinationWaypointLatitude']+rmbData['DestinationWaypointLatitudeNS']+rmbData['DestinationWaypointLongitude']+rmbData['DestinationWaypointLongitudeEW']
+        if rmb_text != self.__RMB_text:
+            # 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
+            # Compute 5 waypoints
+            currentPosition = GeoPoint( str(float(self.__lat)/100)+self.__latNS,
+                                        str(float(self.__lon)/100)+self.__lonEW)
+            destinationPosition = GeoPoint( str(float(rmbData['DestinationWaypointLatitude'])/100)+rmbData['DestinationWaypointLatitudeNS'],
+                                            str(float(rmbData['DestinationWaypointLongitude'])/100)+rmbData['DestinationWaypointLongitudeEW'])
+            self.__RMB_waypoints = self.__GC.getIntermediateWaypoints(currentPosition, destinationPosition, 5)['waypoints']
+            
 
     def __RMC_sender(self):
+        
+        # Move to the next intermediate waypoint
+        if self.__RMB_iteration != None:
+            n = self.__RMB_iteration
+            target = self.__RMB_waypoints[0]
+            targetPoint = GeoPoint(target['latitude'], target['longitude'])
+            self.__lat, self.__latNS, self.__lon, self.__lonEW = targetPoint.toNMEA()
+            self.__RMB_iteration += 1
+
         now = datetime.datetime.now()
         return self.__RMC.encodeDict( { 'UtcOfPositionFix': 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': now.strftime('%d%m%y'),
                                         'MagneticVariation': '0.0',
                                         'ModeIndicator': 'E'
                                     } )
 
     def __init__(self):
         # 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, 3)
 
     def start(self):
         self.__NT.startTransceiver()
 
 #
 # Main
 #
 
 TB = TestBoat()
 TB.start()
 
 # EOF