From 49dd5e295ff6db3ee6f416ffb66855d0ac6b7232 Mon Sep 17 00:00:00 2001
From: Henry Winkel <henrywinkel@bundeswehr.org>
Date: Thu, 2 Nov 2023 11:05:42 +0100
Subject: [PATCH] ADD: added M/S Knots conversion and correction functions for
 course and clb

---
 include/SimCore/UtilFunctions.hpp | 13 +++++++++++
 src/SimCore/UtilFunctions.cpp     | 39 +++++++++++++++++++++++++++++++
 2 files changed, 52 insertions(+)

diff --git a/include/SimCore/UtilFunctions.hpp b/include/SimCore/UtilFunctions.hpp
index 13fa6be..86d7585 100644
--- a/include/SimCore/UtilFunctions.hpp
+++ b/include/SimCore/UtilFunctions.hpp
@@ -12,6 +12,8 @@
 namespace SimCore {
 
 
+#define __MS_TO_KNOTS__ 1.943844;
+
     class UtilFunctions
     {
         public:
@@ -27,9 +29,20 @@ namespace SimCore {
             static double DegToRad(double deg);
             static double RadToDeg(double rad);
 
+            static double MSToKnots(double ms); 
+            static double KnotsToMs(double knots);
+
+
+            static double ShiftRadCrs(double Rad);
+            static double ShiftRadClb(double Rad);
+
+       
+    
     };
 
 
+
+
 class AsyncWorker {
 	std::atomic<bool> active{true};
 	
diff --git a/src/SimCore/UtilFunctions.cpp b/src/SimCore/UtilFunctions.cpp
index ff27ca8..98716f5 100644
--- a/src/SimCore/UtilFunctions.cpp
+++ b/src/SimCore/UtilFunctions.cpp
@@ -103,4 +103,43 @@ namespace SimCore {
         return rad * 180 / M_PI;
      }
 
+    double UtilFunctions::MSToKnots(double ms)
+    {
+        return ms * __MS_TO_KNOTS__;
+    } 
+    double UtilFunctions::KnotsToMs(double knots)
+    {
+        return knots / __MS_TO_KNOTS__;
+    }
+
+
+
+     double UtilFunctions::ShiftRadCrs(double Rad)
+        {
+            while(Rad < 0)
+            {
+                Rad += 2 * M_PI;
+            }
+
+            while (Rad > 2 * M_PI) 
+            {
+            Rad -= 2 * M_PI;
+            }
+            return Rad;
+        };
+
+        double UtilFunctions::ShiftRadClb(double Rad)
+        {
+            while(Rad < -M_PI)
+            {
+                Rad += 2 * M_PI;
+            }
+
+            while (Rad >  M_PI) 
+            {
+                Rad -= 2 * M_PI;
+            }
+        return Rad;
+        };
+
 }
\ No newline at end of file