GPS4LPV.py

"""
Author: chenb@pfw.edu

Date: 4/23/2024

Description:
  GPS related functions for LPV

  Distance between two locations: haversine algorithm
  https://github.com/mapado/haversine

  
"""

from sparkfungps import SparkfunGPS
from haversine import Unit, haversine, haversine_vector


class Boat_GPS:
  """ Use GPS to determine LPV position, speed, heading and distance to the gate
  """
  def __init__(self, buoys):
      """Args: 
          buoys:  a list of buoys. each buoy has a position tuple in (latitude,longitude).
                  [(latitude, longitude), (latitude, longitude)...] in degrees
      """
      self.gps = SparkfunGPS(port='/dev/ttyACM0', baudrate=115200) # init gps
      self.buoys = buoys

  def lpv2buoys(self):
      """ get distance between LPV and the middle of the gate

        Output: distance in meters
      """
      lat, lon, heading = self.gps.get_latitude_longitude_heading()
      distances = []
      for pos in self.buoys:
        distances.append(haversine((lat, lon), pos, unit=Unit.METERS))
      return distances, heading
  
  # LPV speed, heading, acc from get_reckoning()
  

if __name__ == "__main__":
   
   # test: US cities
   # https://www.latlong.net/category/cities-236-15.html
   #  (Valparaiso), (East Chicago), (Houston)
   city_locations = [(41.483845,	-87.063965), (41.649212,	-87.472565), (29.749907,	-95.358421)]
   # fort wayne
   lpv =(41.0793, -85.1394)

   distances = []
   for pos in city_locations:
      distances.append(haversine(lpv, pos, unit=Unit.METERS))
   print(distances)