From 36ef3b310c09cd7fd8980b7d43e6aa26d4aa5b2d Mon Sep 17 00:00:00 2001
From: Marco <49691247+marcoSchr@users.noreply.github.com>
Date: Sun, 31 Mar 2024 11:56:49 +0200
Subject: [PATCH] Added function to convert minmea coordinate to fixed-point
 integer representation

---
 .github/workflows/main.yml        |  2 ++
 lib/minmea/include/minmea.h       |  6 ++++++
 lib/minmea/minmea.c               | 16 ++++++++++++++
 meson.build                       |  5 +++++
 openrtx/include/core/gps.h        |  2 ++
 tests/unit/convert_minmea_coord.c | 35 +++++++++++++++++++++++++++++++
 6 files changed, 66 insertions(+)
 create mode 100644 tests/unit/convert_minmea_coord.c

diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index b48234ef..49ea1ce9 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -80,6 +80,8 @@ jobs:
         run: meson test -C build "M17 RRC Test"
       - name: Codeplug Test
         run: meson test -C build "Codeplug Test"
+      - name: minmea Conversion Test
+        run: meson test -C build "minmea conversion Test"
       # The following tests are disabled because they appear to be flakey when run in CI
       # - name: Sine Test
       #   run: meson test -C build "Sine Test"
diff --git a/lib/minmea/include/minmea.h b/lib/minmea/include/minmea.h
index 181d8c7c..187235d4 100644
--- a/lib/minmea/include/minmea.h
+++ b/lib/minmea/include/minmea.h
@@ -252,6 +252,12 @@ static inline float minmea_tocoord(struct minmea_float *f)
     return (float) degrees + (float) minutes / (60 * f->scale);
 }
 
+/**
+ * Convert a raw coordinate to a fixed point value.
+ * Returns zero for "unknown" values.
+ */
+int minmea_tofixedpoint(struct minmea_float *f);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/minmea/minmea.c b/lib/minmea/minmea.c
index 5b311ce6..e2806872 100644
--- a/lib/minmea/minmea.c
+++ b/lib/minmea/minmea.c
@@ -642,4 +642,20 @@ int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const st
     }
 }
 
+int minmea_tofixedpoint(struct minmea_float *f) {
+
+    if (f->scale == 0)
+        return 0;
+
+    int32_t value = f->value;
+    int32_t scale = f->scale;
+    int8_t sign = value < 0 ? -1 : 1;
+    // Ensure value is always positive
+    value = value * sign;
+    int32_t coord_int = value / (scale * 100);
+    int32_t coord_dec = (value % (scale * 100)) * (100000 / scale) / 6;
+    return (coord_int * 1000000 + coord_dec) * sign;
+}
+
+
 /* vim: set ts=4 sw=4 et: */
diff --git a/meson.build b/meson.build
index a75778f3..cb1ed596 100644
--- a/meson.build
+++ b/meson.build
@@ -880,6 +880,10 @@ vp_test = executable('vp_test',
                       sources : unit_test_src + ['tests/unit/voice_prompts.c'],
                       kwargs  : unit_test_opts)
 
+minmea_conversion_test = executable('minmea_conversion_test',
+                                     sources : unit_test_src + ['tests/unit/convert_minmea_coord.c'],
+                                     kwargs  : unit_test_opts)
+
 test('M17 Golay Unit Test',   m17_golay_test)
 test('M17 Viterbi Unit Test', m17_viterbi_test)
 ## test('M17 Demodulator Test',  m17_demodulator_test) # Skipped for now as this test no longer works after an M17 refactor
@@ -888,3 +892,4 @@ test('Codeplug Test',         cps_test)
 test('Linux InputStream Test', linux_inputStream_test)
 test('Sine Test',             sine_test)
 ## test('Voice Prompts Test',    vp_test) # Skipped for now as this test no longer works
+test('minmea conversion Test', minmea_conversion_test)
diff --git a/openrtx/include/core/gps.h b/openrtx/include/core/gps.h
index 6c610be0..5607314f 100644
--- a/openrtx/include/core/gps.h
+++ b/openrtx/include/core/gps.h
@@ -22,6 +22,7 @@
 
 #include <datetime.h>
 #include <stdint.h>
+#include <minmea.h>
 
 /**
  * Data structure representing a single satellite as part of a GPS fix.
@@ -63,4 +64,5 @@ gps_t;
  */
 void gps_task();
 
+
 #endif /* GPS_H */
diff --git a/tests/unit/convert_minmea_coord.c b/tests/unit/convert_minmea_coord.c
new file mode 100644
index 00000000..fe01793f
--- /dev/null
+++ b/tests/unit/convert_minmea_coord.c
@@ -0,0 +1,35 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <gps.h>
+
+static void assert_conversion(struct minmea_float *f, int32_t expected)
+{
+    int32_t result = minmea_tofixedpoint(f);
+    if (result != expected)
+    {
+        printf("FAILED! result value %d - expected %d\n",
+               result, expected);
+        exit(1);
+    }
+}
+
+int main() {
+    printf("minmea coordinate conversion test\n");
+    struct minmea_float test = {5333735, 1000};
+    assert_conversion(&test, 53562250);
+    test.scale = 1;
+    test.value = 0;
+    assert_conversion(&test, 0);
+    test.scale = 1000;
+    test.value = -5333735;
+    assert_conversion(&test, -53562250);
+    test.scale = 1000;
+    test.value = -330;
+    assert_conversion(&test, -5500);
+    test.scale = 1000;
+    test.value = -3296;
+    assert_conversion(&test, -54933);
+    printf("PASS\n");
+
+    return 0;
+}