From e55a3782a5c1021b3d2442dcd5ba86f213e83a25 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Thu, 17 Dec 2020 12:32:13 +0000
Subject: [PATCH] GPS fixed

---
 src/cam.c        | 26 ++++++++++++++++----------
 src/config.c     | 33 ++++++++++++++++++++++++++++-----
 src/facilities.h |  4 ++++
 3 files changed, 48 insertions(+), 15 deletions(-)

diff --git a/src/cam.c b/src/cam.c
index d1b4025..88091d2 100644
--- a/src/cam.c
+++ b/src/cam.c
@@ -155,17 +155,23 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
        cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
    }
 
-   if (!isnan(gps_data.gps_latitude)) {
-       cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000);
-   } else {
-       cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable;
-   }
+    if (!facilities->gps_fixed) {
+        if (!isnan(gps_data.gps_latitude)) {
+            cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000);
+        } else {
+            cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable;
+        }
 
-   if (!isnan(gps_data.gps_longitude)) {
-       cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000);
-   } else {
-       cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable;
-   }
+        if (!isnan(gps_data.gps_longitude)) {
+            cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000);
+        } else {
+            cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable;
+        }
+    } else {
+        cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000);
+        cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000);
+    
+    }
 
     cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
     cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
diff --git a/src/config.c b/src/config.c
index ab6a8bf..06e08d0 100644
--- a/src/config.c
+++ b/src/config.c
@@ -63,6 +63,21 @@ static int extract_val_int(int64_t * output, toml_table_t *table, char* name) {
     return 0;
 }
 
+static int extract_val_double(double * output, toml_table_t *table, char* name) {
+    toml_raw_t raw;
+    if (0 == (raw = toml_raw_in(table, name))) {
+        syslog_err("[itss] [config] error extracting %s from file", name);
+        return 1;
+    }
+
+    if (toml_rtod(raw, output)) {
+        syslog_err("[itss] [config] error extracting %s to double", name);
+        return 1;
+    }
+
+    return 0;
+}
+
 int itss_config(void* facilities_s, char* config_file) {
     int rv = 0;
 
@@ -88,7 +103,7 @@ int itss_config(void* facilities_s, char* config_file) {
     fclose(fp);
 
     // Tables
-    toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim, *protected_zones;
+    toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim, *protected_zones, *gps;
 
     if (0 == (general = toml_table_in(conf, "general"))) {syslog_err("[facilities] [config] failed locating [general] table"); return 1;}
     if (0 == (security = toml_table_in(conf, "security"))) {syslog_err("[facilities] [config] failed locating [security] table"); return 1;}
@@ -97,8 +112,7 @@ int itss_config(void* facilities_s, char* config_file) {
    if (0 == (replay = toml_table_in(conf, "replay"))) {syslog_err("[facilities] [config] failed locating [replay] table"); return 1;}
    if (0 == (ivim = toml_table_in(conf, "ivim"))) {syslog_err("[facilities] [config] failed locating [ivim] table"); return 1;}
    if (0 == (protected_zones = toml_table_in(conf, "protected-zones"))) {syslog_err("[facilities] [config] failed locating [protected-zones] table"); return 1;}
-
-
+   if (0 == (gps = toml_table_in(conf, "gps"))) {syslog_err("[facilities] [config] failed locating [gps] table"); return 1;}
 
     // Values
     // General
@@ -222,9 +236,18 @@ int itss_config(void* facilities_s, char* config_file) {
         free(pz_path);
     }
 
-    
-
+    int gps_fixed = 0;
+    rv += extract_val_bool(&gps_fixed, gps, "fixed");
+    facilities->gps_fixed = gps_fixed;
+    if (gps_fixed) {
+        double latitude;
+        rv += extract_val_double(&latitude, gps, "latitude");
+        facilities->latitude = latitude;
 
+        double longitude;
+        rv += extract_val_double(&longitude, gps, "longitude");
+        facilities->longitude = longitude;
+    }
 
     toml_free(conf);
 
diff --git a/src/facilities.h b/src/facilities.h
index 3b7b4de..685571e 100644
--- a/src/facilities.h
+++ b/src/facilities.h
@@ -45,6 +45,10 @@ typedef struct facilities {
     uint64_t station_id;
     bool id_random;
 
+    bool gps_fixed;
+    double latitude;
+    double longitude;
+
     bool exit;
 } facilities_t;