From 33e5eba3a65f94a73c419cfecd177ef775196878 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Thu, 14 Dec 2023 23:29:09 +0100
Subject: [PATCH] Added first simple GPS implementation

---
 demo.ipynb                    | 18 +++++++-----------
 spatz/sensors/gps/erinome1.py | 17 +++++++++++++++++
 spatz/sensors/gps/gps.py      | 33 +++++++++++++++++++++++++++++++++
 3 files changed, 57 insertions(+), 11 deletions(-)
 create mode 100644 spatz/sensors/gps/erinome1.py
 create mode 100644 spatz/sensors/gps/gps.py

diff --git a/demo.ipynb b/demo.ipynb
index 890c67c..febee06 100644
--- a/demo.ipynb
+++ b/demo.ipynb
@@ -88,12 +88,16 @@
    "source": [
     "from spatz.sensors.imu.wsen_isds import WSEN_ISDS_ACC, WSEN_ISDS_GYRO\n",
     "from spatz.sensors.pressure.ms5611_01ba03 import MS5611_01BA03\n",
+    "from spatz.sensors.gps.erinome1 import Erinome_I\n",
     "\n",
     "press_sensor = simulation.add_sensor(MS5611_01BA03)\n",
     "\n",
     "# Use the offset argument to change the position of the imu in relation to the rocket's center of gravity.\n",
     "accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, offset=0)\n",
-    "gyro = simulation.add_sensor(WSEN_ISDS_GYRO, offset=0)"
+    "gyro = simulation.add_sensor(WSEN_ISDS_GYRO, offset=0)\n",
+    "\n",
+    "# Add a GPS module to the simulation which returns the following data: [latitude, longitude, altitude (km)]\n",
+    "gps_module = simulation.add_sensor(Erinome_I)"
    ]
   },
   {
@@ -171,10 +175,6 @@
     }
    ],
    "source": [
-    "from spatz.metrics import Metric\n",
-    "\n",
-    "metric = Metric()\n",
-    "\n",
     "logger = simulation.get_logger()\n",
     "\n",
     "# Set verbose to False to disable the progress bar\n",
@@ -183,16 +183,12 @@
     "    press = press_sensor()\n",
     "    acc = accelerometer()\n",
     "    rot_rate = gyro()\n",
+    "    gps = gps_module()\n",
     "\n",
     "    # Get the correct altitude data.\n",
     "    alt = altitude()\n",
     "\n",
-    "    pred_alt = ...\n",
-    "    metric(alt, pred_alt)\n",
-    "\n",
-    "    # TODO: Add your computation here.\n",
-    "\n",
-    "print('Score was:', metric.get_score())"
+    "    # TODO: Add your computation here."
    ]
   },
   {
diff --git a/spatz/sensors/gps/erinome1.py b/spatz/sensors/gps/erinome1.py
new file mode 100644
index 0000000..9a9483b
--- /dev/null
+++ b/spatz/sensors/gps/erinome1.py
@@ -0,0 +1,17 @@
+from typing import List
+
+from numpy.typing import ArrayLike
+from sensors.gps import GPS
+from spatz.dataset import ArrayLike, Dataset
+from spatz.logger import ArrayLike, Logger
+from spatz.transforms import Transform
+
+
+class Erinome_I(GPS):
+    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+        super().__init__(dataset, logger, transforms)
+
+    def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
+        # TODO: What's the GPS module's behavior?
+
+        return x
\ No newline at end of file
diff --git a/spatz/sensors/gps/gps.py b/spatz/sensors/gps/gps.py
new file mode 100644
index 0000000..99b58e6
--- /dev/null
+++ b/spatz/sensors/gps/gps.py
@@ -0,0 +1,33 @@
+from typing import Any, List
+from numpy.linalg import norm
+
+from numpy.typing import ArrayLike
+from spatz.dataset import ArrayLike, Dataset
+from spatz.logger import ArrayLike, Logger
+from spatz.sensors import Sensor, NA
+from spatz.transforms import Transform
+
+# WG84 googlen (world model GPS)
+       
+
+
+class GPS(Sensor):
+    def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
+        """GPS Module which provides the following information:
+        - Longitude (in °)
+        - Latitiude (in °)
+        - Altitude (in °)
+        """
+        super().__init__(dataset, logger, transforms)
+
+    def _get_data(self) -> ArrayLike:
+        vel = norm(self._dataset.get_velocity())
+
+        # TODO: At which speed do we assume that GPS becomes unreliable?
+        if vel / self._dataset.get_mach_number() > 1:
+            return NA
+        
+        x = self._dataset.fetch_values(['latitude', 'longitude', 'altitude'])
+        x = self._sensor_specific_effects(x)
+
+        return x