minindn/apps/gpsd.py

# -*- Mode:python; c-file-style:"gnu"; indent-tabs-mode:nil -*- */
#
# Copyright (C) 2015-2025, The University of Memphis,
#                          Arizona Board of Regents,
#                          Regents of the University of California.
#
# This file is part of Mini-NDN.
# See AUTHORS.md for a complete list of Mini-NDN authors and contributors.
#
# Mini-NDN is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# Mini-NDN is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with Mini-NDN, e.g., in COPYING.md file.
# If not, see <http://www.gnu.org/licenses/>.

import threading
import math
import time
import datetime
from subprocess import TimeoutExpired, DEVNULL
from typing import Optional, Tuple

from mininet.log import error

from minindn.apps.application import Application
from minindn.util import getPopen


class Gpsd(Application):
    """
    A class that simulates a GPS device and provides GPS data in NMEA format.
    This class can generate NMEA GGA and VTG sentences and feeds the GPS data to a GPSD server.

    To use this class, you need to install gpsd and netcat (nc) for communication.
    """

    def __init__(self, node, lat: float = 0.0, lon: float = 0.0, altitude: float = 0.0, update_interval: float = 0.2) -> None:
        """
        Initialize the GPSd application for a node.

        :param node: The node for which the GPS data will be provided.
        :param lat: Latitude of the point (0, 0, 0).
        :param lon: Longitude of the point (0, 0, 0).
        :param altitude: Altitude of the point (0, 0, 0).
        :param update_interval: The time interval in seconds to send gps data to gpsd, and it should be more than 0.2
        """
        Application.__init__(self, node)
        self.lat = lat
        self.lon = lon
        self.altitude = altitude
        self.update_interval = update_interval
        self.stop_event = threading.Event()
        self.location_thread = None


    def calculate_coordinates(self, offset_lat: float, offset_lon: float, altitude_offset: float) -> Tuple[float, float, float]:
        """
        Calculate the coordinates of the target point.

        :param offset_lat: Latitude offset (unit: meters)
        :param offset_lon: Longitude offset (unit: meters)
        :param altitude_offset: Altitude offset (unit: meters)
        :return: New GPS coordinates (latitude, longitude, altitude)
        """
        # Each degree of latitude is approximately 111 kilometers
        lat_offset_deg = offset_lat / 111000.0  # Convert to degrees
        # Distance per degree of longitude changes based on latitude
        lon_offset_deg = offset_lon / (111000.0 * math.cos(math.radians(self.lat)))  # Convert to degrees
        # Calculate the latitude, longitude, and altitude of the target point
        lat2 = self.lat + lat_offset_deg
        lon2 = self.lon + lon_offset_deg
        alt2 = self.altitude + altitude_offset

        return lat2, lon2, alt2

    @staticmethod
    def nmea_checksum(sentence: str) -> str:
        """
        Calculate the checksum for an NMEA sentence.
        :param sentence: The NMEA sentence to calculate the checksum for.
        :return: The checksum for the sentence.
        """
        checksum = 0
        for char in sentence:
            checksum ^= ord(char)
        return f"{checksum:02X}"

    @staticmethod
    def generate_vtg_sentence(vx: float, vy: float) -> str:
        """
        Generate a NMEA VTG sentence based on a 2D velocity vector.

        Parameters:
            vx (float): Velocity in the x-direction (eastward, in m/s)
            vy (float): Velocity in the y-direction (northward, in m/s)

        Returns:
            str: The corresponding NMEA VTG sentence
        """
        # Calculate ground speed (horizontal speed)
        ground_speed = math.sqrt(vx**2 + vy**2)  # Ground speed in m/s
        ground_speed_knots = ground_speed * 1.94384  # Convert speed to knots
        ground_speed_kmh = ground_speed * 3.6       # Convert speed to km/h
        # Calculate heading angle (relative to true north, clockwise)
        angle = math.atan2(vx, vy)  # atan2(y, x)
        true_course = (math.degrees(angle) + 360) % 360  # Normalize angle to 0° - 360°
        # Create the VTG NMEA sentence
        nmea_sentence = f"GPVTG,{true_course:.1f},T,,M,{ground_speed_knots:.2f},N,{ground_speed_kmh:.2f},K"

        nmea_sentence_with_checksum = f"${nmea_sentence}*{Gpsd.nmea_checksum(nmea_sentence)}"

        return nmea_sentence_with_checksum

    @staticmethod
    def generate_gga_sentence(lat: float, lon: float, altitude: float, utc_time: Optional[str] = None) -> str:
        """
        Convert latitude, longitude, and altitude into NMEA format data.

        Parameters:
        lat (float): Latitude (unit: degrees)
        lon (float): Longitude (unit: degrees)
        altitude (float): Altitude (unit: meters)
        utc_time (str): UTC Time in 'hhmmss.sss' format, optional

        Returns:
        str: Formatted NMEA data sentence
        """
        lat_direction = 'N' if lat >= 0 else 'S'
        lon_direction = 'E' if lon >= 0 else 'W'

        lat_deg = int(abs(lat))
        lat_min = (abs(lat) - lat_deg) * 60
        lon_deg = int(abs(lon))
        lon_min = (abs(lon) - lon_deg) * 60

        if utc_time is None:
            utc_time = datetime.datetime.now(datetime.timezone.utc).strftime("%H%M%S.%f")[:-3]# Generate UTC time

        nmea_sentence = f"GPGGA,{utc_time},{lat_deg:02d}{lat_min:07.4f},{lat_direction},{lon_deg:03d}{lon_min:07.4f},{lon_direction},1,12,1.0,{altitude:.1f},M,0.0,M,,"

        nmea_sentence = f"${nmea_sentence}*{Gpsd.nmea_checksum(nmea_sentence)}"

        return nmea_sentence

    @staticmethod
    def generate_rmc_sentence(lat: float, lon: float, vx: float, vy: float, utc_time: Optional[str] = None, date: Optional[str] = None) -> str:
        """
        Generate an NMEA GPRMC sentence using vx and vy (speed components along X and Y axes).

        Parameters:
        lat (float): Latitude (degrees)
        lon (float): Longitude (degrees)
        vx (float): Speed along the X-axis (knots)
        vy (float): Speed along the Y-axis (knots)
        utc_time (str, optional): UTC time in 'hhmmss.sss' format
        date (str, optional): UTC date in 'ddmmyy' format

        Returns:
        str: Formatted NMEA GPRMC sentence
        """
        # Calculate speed and course
        speed = math.sqrt(vx**2 + vy**2)  # Speed (ground speed)
        course = math.degrees(math.atan2(vy, vx))  # Course (direction in degrees)

        # Normalize course to be within 0 to 360 degrees
        if course < 0:
            course += 360

        # Convert latitude and longitude to NMEA format
        lat_direction = 'N' if lat >= 0 else 'S'
        lon_direction = 'E' if lon >= 0 else 'W'

        lat_deg = int(abs(lat))
        lat_min = (abs(lat) - lat_deg) * 60
        lon_deg = int(abs(lon))
        lon_min = (abs(lon) - lon_deg) * 60

        # Use current utc_time and date if not provided
        if utc_time is None or date is None:
            now = datetime.datetime.now(datetime.timezone.utc)
            if utc_time is None:
                utc_time = now.strftime("%H%M%S.%f")[:-3]  # hhmmss.sss
            if date is None:
                date = now.strftime("%d%m%y")  # ddmmyy

        # Construct NMEA sentence
        status = "A"  # A = Active, V = Void (No fix)
        nmea_sentence = f"GPRMC,{utc_time},{status},{lat_deg:02d}{lat_min:07.4f},{lat_direction},{lon_deg:03d}{lon_min:07.4f},{lon_direction},{speed:.1f},{course:.1f},{date},,,A"

        nmea_sentence = f"${nmea_sentence}*{Gpsd.nmea_checksum(nmea_sentence)}"

        return nmea_sentence

    def __feedGPStoGPSD(self, node) -> None:
        """
        Continuously feed GPS data to the GPSD server.
        """
        current_position = node.position
        current_time_seconds = time.monotonic()
        while not self.stop_event.is_set():
            time.sleep(self.update_interval)
            lat, lon, altitude = self.calculate_coordinates(node.position[0], node.position[1], node.position[2])
            gga_sentence = self.generate_gga_sentence(lat, lon, altitude)

            tmp_position = node.position
            tmp_time_seconds = time.monotonic()
            vx = (tmp_position[0] - current_position[0]) / (tmp_time_seconds - current_time_seconds)
            vy = (tmp_position[1] - current_position[1]) / (tmp_time_seconds - current_time_seconds)
            current_position = tmp_position
            current_time_seconds = tmp_time_seconds
            rmc_sentence = self.generate_rmc_sentence(lat, lon, vx, vy)
            vtg_sentence = self.generate_vtg_sentence(vx, vy)

            cmd = f"echo '{gga_sentence}\n{rmc_sentence}\n{vtg_sentence}\n' | nc -u -w 1 127.0.0.1 7150"
            process = node.popen(cmd, shell=True)

    def start(self) -> None:
        """
        Start a thread to periodically send GPS data for the node.
        """
        try:
            gpsd_present = getPopen(self.node, "gpsd -V", stdout=DEVNULL, stderr=DEVNULL).wait(timeout=5)
            nc_present = getPopen(self.node, "nc -h", stdout=DEVNULL, stderr=DEVNULL).wait(timeout=5)
            if gpsd_present > 0:
                error("The application is currently missing gpsd as a dependency. This must be installed manually.\n")
            elif nc_present > 0:
                error("The application is currently missing netcat as a dependency. This must be installed manually.\n")
            if (gpsd_present + nc_present) > 0:
                raise RuntimeError("Missing dependency for gpsd helper")
        except TimeoutExpired as e:
            error("The application is unable to validate if you have all required dependencies for gpsd.\n")
            raise e

        Application.start(self, command="gpsd -n udp://127.0.0.1:7150")

        self.location_thread = threading.Thread(target=self.__feedGPStoGPSD, args=(self.node,))
        self.location_thread.start()

    def stop(self) -> None:
        """
        Stop all the __feedGPStoGPSD threads
        """
        if not self.stop_event.is_set():
            self.stop_event.set()
        self.location_thread.join()
        Application.stop(self)