From 047ced2b20d1ce3ccad501a8cf553f4395723689 Mon Sep 17 00:00:00 2001
From: Carles Fernandez <carles.fernandez@gmail.com>
Date: Mon, 3 Oct 2016 13:43:20 +0200
Subject: [PATCH] Improve queue management

---
 src/core/receiver/control_thread.cc   | 14 ++++++++++----
 src/tests/system-tests/ttff_gps_l1.cc |  9 +++------
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc
index 7a5e9a371..a0fe84d23 100644
--- a/src/core/receiver/control_thread.cc
+++ b/src/core/receiver/control_thread.cc
@@ -34,7 +34,9 @@
 
 #include "control_thread.h"
 #include <unistd.h>
+#include <cmath>
 #include <iostream>
+#include <limits>
 #include <map>
 #include <string>
 #include <sys/types.h>
@@ -528,16 +530,18 @@ void ControlThread::sysv_queue_listener()
     int msgrcv_size = sizeof(msg.ttff);
     int msqid;
     key_t key = 1102;
-    // wait for the queue to be created
+
+    if((msqid = msgget(key, 0644 | IPC_CREAT )) == -1)
+        {
+            perror("msgget");
+        }
 
     while(read_queue)
         {
-            while((msqid = msgget(key, 0644)) == -1){}
             if (msgrcv(msqid, &msg, msgrcv_size, 1, 0) != -1)
                 {
-
                     ttff_msg = msg.ttff;
-                    if(ttff_msg == 200)
+                    if( (std::abs(ttff_msg - (-200.0)) < 10 * std::numeric_limits<double>::epsilon()) )
                         {
                             std::cout << "Quit order received, stopping GNSS-SDR !!" << std::endl;
                             std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
@@ -548,7 +552,9 @@ void ControlThread::sysv_queue_listener()
                             read_queue = false;
                         }
                 }
+            usleep(1000000);
         }
+    msgctl(msqid, IPC_RMID, NULL);
 }
 
 
diff --git a/src/tests/system-tests/ttff_gps_l1.cc b/src/tests/system-tests/ttff_gps_l1.cc
index de01adbdc..50ce4553b 100644
--- a/src/tests/system-tests/ttff_gps_l1.cc
+++ b/src/tests/system-tests/ttff_gps_l1.cc
@@ -248,7 +248,7 @@ void receive_msg()
     ttff_msgbuf msg;
     ttff_msgbuf msg_stop;
     msg_stop.mtype = 1;
-    msg_stop.ttff = 200;
+    msg_stop.ttff = -200.0;
     double ttff_msg = 0.0;
     int msgrcv_size = sizeof(msg.ttff);
     int msqid;
@@ -266,7 +266,8 @@ void receive_msg()
                     TTFF_v.push_back(ttff_msg / (1000.0 / decimation_factor) );
                     LOG(INFO) << "Valid Time-To-First-Fix: " << ttff_msg / (1000.0 / decimation_factor ) << "[s]";
                     // Stop the receiver
-                    while(((msqid_stop = msgget(key_stop, 0644 | IPC_CREAT))) == -1){}
+                    //while(((msqid_stop = msgget(key_stop, 0644 | IPC_CREAT))) == -1){}
+                    while(((msqid_stop = msgget(key_stop, 0644))) == -1){}
                     double msgsend_size = sizeof(msg_stop.ttff);
                     msgsnd(msqid_stop, &msg_stop, msgsend_size, IPC_NOWAIT);
                 }
@@ -275,10 +276,6 @@ void receive_msg()
                 {
                     receive_msg();
                 }
-            else
-                {
-                    if(msqid_stop != -1) msgctl(msqid_stop, IPC_RMID, NULL);
-                }
         }
     return;
 }