Merge branch 'next' into tii

13 files changed, 603 insertions, 588 deletions

diff --git a/src/DabMod.cpp b/src/DabMod.cpp
index 9f096ab..1fc7e3c 100644
--- a/src/DabMod.cpp
+++ b/src/DabMod.cpp
@@ -157,10 +157,8 @@ int launch_modulator(int argc, char* argv[])
     modulator_data m;
 
     // To handle the timestamp offset of the modulator
-    struct modulator_offset_config modconf;
-    modconf.use_offset_file = false;
-    modconf.use_offset_fixed = false;
-    modconf.delay_calculation_pipeline_stages = 0;
+    unsigned tist_delay_stages = 0;
+    double   tist_offset_s = 0.0;
 
     shared_ptr<Flowgraph> flowgraph(new Flowgraph());
     shared_ptr<FormatConverter> format_converter;
@@ -238,25 +236,7 @@ int launch_modulator(int argc, char* argv[])
             loop = true;
             break;
         case 'o':
-            if (modconf.use_offset_file)
-            {
-                fprintf(stderr, "Options -o and -O are mutually exclusive\n");
-                throw std::invalid_argument("Invalid command line options");
-            }
-            modconf.use_offset_fixed = true;
-            modconf.offset_fixed = strtod(optarg, NULL);
-#if defined(HAVE_OUTPUT_UHD)
-            outputuhd_conf.enableSync = true;
-#endif
-            break;
-        case 'O':
-            if (modconf.use_offset_fixed)
-            {
-                fprintf(stderr, "Options -o and -O are mutually exclusive\n");
-                throw std::invalid_argument("Invalid command line options");
-            }
-            modconf.use_offset_file = true;
-            modconf.offset_filename = std::string(optarg);
+            tist_offset_s = strtod(optarg, NULL);
 #if defined(HAVE_OUTPUT_UHD)
             outputuhd_conf.enableSync = true;
 #endif
@@ -578,23 +558,20 @@ int launch_modulator(int argc, char* argv[])
 #if defined(HAVE_OUTPUT_UHD)
         outputuhd_conf.enableSync = (pt.get("delaymanagement.synchronous", 0) == 1);
         if (outputuhd_conf.enableSync) {
+            std::string delay_mgmt = pt.get<std::string>("delaymanagement.management", "");
+            std::string fixedoffset = pt.get<std::string>("delaymanagement.fixedoffset", "");
+            std::string offset_filename = pt.get<std::string>("delaymanagement.dynamicoffsetfile", "");
+
+            if (not(delay_mgmt.empty() and fixedoffset.empty() and offset_filename.empty())) {
+                std::cerr << "Warning: you are using the old config syntax for the offset management.\n";
+                std::cerr << "         Please see the example.ini configuration for the new settings.\n";
+            }
+
             try {
-                std::string delay_mgmt = pt.get<std::string>("delaymanagement.management");
-                if (delay_mgmt == "fixed") {
-                    modconf.offset_fixed = pt.get<double>("delaymanagement.fixedoffset");
-                    modconf.use_offset_fixed = true;
-                }
-                else if (delay_mgmt == "dynamic") {
-                    modconf.offset_filename = pt.get<std::string>("delaymanagement.dynamicoffsetfile");
-                    modconf.use_offset_file = true;
-                }
-                else {
-                    throw std::runtime_error("invalid management value");
-                }
+                tist_offset_s = pt.get<double>("delaymanagement.offset");
             }
             catch (std::exception &e) {
-                std::cerr << "Error: " << e.what() << "\n";
-                std::cerr << "       Synchronised transmission enabled, but delay management specification is incomplete.\n";
+                std::cerr << "Error: delaymanagement: synchronous is enabled, but no offset defined!\n";
                 throw std::runtime_error("Configuration error");
             }
         }
@@ -611,16 +588,10 @@ int launch_modulator(int argc, char* argv[])
 
     etiLog.level(info) << "Starting up";
 
-    if (!(modconf.use_offset_file || modconf.use_offset_fixed)) {
-        etiLog.level(debug) << "No Modulator offset defined, setting to 0";
-        modconf.use_offset_fixed = true;
-        modconf.offset_fixed = 0;
-    }
-
     // When using the FIRFilter, increase the modulator offset pipelining delay
     // by the correct amount
     if (filterTapsFilename != "") {
-        modconf.delay_calculation_pipeline_stages += FIRFILTER_PIPELINE_DELAY;
+        tist_delay_stages += FIRFILTER_PIPELINE_DELAY;
     }
 
     // Setting ETI input filename
@@ -768,15 +739,6 @@ int launch_modulator(int argc, char* argv[])
     }
 #endif
 
-    // Set thread priority to realtime
-    const int policy = SCHED_RR;
-    sched_param sp;
-    sp.sched_priority = sched_get_priority_min(policy);
-    int thread_prio_ret = pthread_setschedparam(pthread_self(), policy, &sp);
-    if (thread_prio_ret != 0) {
-        etiLog.level(error) << "Could not set priority for Modulator thread:" << thread_prio_ret;
-    }
-
 
     while (run_again) {
         Flowgraph flowgraph;
@@ -786,8 +748,9 @@ int launch_modulator(int argc, char* argv[])
 
         shared_ptr<InputMemory> input(new InputMemory(&m.data));
         shared_ptr<DabModulator> modulator(
-                new DabModulator(modconf, &rcs, outputRate, clockRate,
-                    dabMode, gainMode, digitalgain, normalise, filterTapsFilename));
+                new DabModulator(tist_offset_s, tist_delay_stages, &rcs,
+                    outputRate, clockRate, dabMode, gainMode, digitalgain,
+                    normalise, filterTapsFilename));
 
         flowgraph.connect(input, modulator);
         if (format_converter) {
diff --git a/src/DabModulator.cpp b/src/DabModulator.cpp
index 0ead555..97c36da 100644
--- a/src/DabModulator.cpp
+++ b/src/DabModulator.cpp
@@ -56,7 +56,7 @@
 using namespace boost;
 
 DabModulator::DabModulator(
-        struct modulator_offset_config& modconf,
+        double tist_offset_s, unsigned tist_delay_stages,
         RemoteControllers* rcs,
         unsigned outputRate, unsigned clockRate,
         unsigned dabMode, GainMode gainMode,
@@ -70,7 +70,7 @@ DabModulator::DabModulator(
     myGainMode(gainMode),
     myDigGain(digGain),
     myNormalise(normalise),
-    myEtiReader(EtiReader(modconf)),
+    myEtiReader(EtiReader(tist_offset_s, tist_delay_stages, rcs)),
     myFlowgraph(NULL),
     myFilterTapsFilename(filterTapsFilename),
     myRCs(rcs)
diff --git a/src/DabModulator.h b/src/DabModulator.h
index 5337f8c..1a9e477 100644
--- a/src/DabModulator.h
+++ b/src/DabModulator.h
@@ -49,7 +49,7 @@ class DabModulator : public ModCodec
 {
 public:
     DabModulator(
-            struct modulator_offset_config& modconf,
+            double tist_offset_s, unsigned tist_delay_stages,
             RemoteControllers* rcs,
             unsigned outputRate = 2048000, unsigned clockRate = 0,
             unsigned dabMode = 0, GainMode gainMode = GAIN_VAR,
diff --git a/src/EtiReader.cpp b/src/EtiReader.cpp
index 100fbdb..f584275 100644
--- a/src/EtiReader.cpp
+++ b/src/EtiReader.cpp
@@ -51,13 +51,18 @@ enum ETI_READER_STATE {
 };
 
 
-EtiReader::EtiReader(struct modulator_offset_config& modconf) :
+EtiReader::EtiReader(
+        double tist_offset_s,
+        unsigned tist_delay_stages,
+        RemoteControllers* rcs) :
     state(EtiReaderStateSync),
     myFicSource(NULL),
-    myTimestampDecoder(modconf)
+    myTimestampDecoder(tist_offset_s, tist_delay_stages)
 {
     PDEBUG("EtiReader::EtiReader()\n");
 
+    myTimestampDecoder.enrol_at(*rcs);
+
     myCurrentFrame = 0;
     eti_fc_valid = false;
 }
@@ -284,11 +289,6 @@ int EtiReader::process(const Buffer* dataIn)
     myTimestampDecoder.updateTimestampEti(eti_fc.FP & 0x3,
             eti_eoh.MNSC, getPPSOffset(), eti_fc.FCT);
 
-    if (eti_fc.FCT % 125 == 0) //every 3 seconds is fine enough
-    {
-        myTimestampDecoder.updateModulatorOffset();
-    }
-
     return dataIn->getLength() - input_size;
 }
 
diff --git a/src/EtiReader.h b/src/EtiReader.h
index 58a1976..84ad9b4 100644
--- a/src/EtiReader.h
+++ b/src/EtiReader.h
@@ -47,7 +47,10 @@
 class EtiReader
 {
 public:
-    EtiReader(struct modulator_offset_config& modconf);
+    EtiReader(
+            double tist_offset_s,
+            unsigned tist_delay_stages,
+            RemoteControllers* rcs);
     virtual ~EtiReader();
     EtiReader(const EtiReader&);
     EtiReader& operator=(const EtiReader&);
@@ -86,8 +89,6 @@ protected:
 
 private:
     size_t myCurrentFrame;
-    bool time_ext_enabled;
-    unsigned long timestamp_seconds;
     bool eti_fc_valid;
 };
 
diff --git a/src/Log.cpp b/src/Log.cpp
index 0464137..9b7a2c3 100644
--- a/src/Log.cpp
+++ b/src/Log.cpp
@@ -73,6 +73,8 @@ void Logger::logstr(log_level_t level, std::string message)
             ++it) {
         (*it)->log(level, message);
     }
+
+    std::cerr << levels_as_str[level] << " " << message << std::endl;
 }
 
 
diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp
index 6ad7dfd..b815a4c 100644
--- a/src/OutputUHD.cpp
+++ b/src/OutputUHD.cpp
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2014
+   Copyright (C) 2014, 2015
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -31,6 +31,7 @@
 #include "PcDebug.h"
 #include "Log.h"
 #include "RemoteControl.h"
+#include "Utils.h"
 
 #include <boost/thread/future.hpp>
 
@@ -60,6 +61,28 @@ void uhd_msg_handler(uhd::msg::type_t type, const std::string &msg)
     }
 }
 
+// Check function for GPS fixtype
+bool check_gps_fix_ok(uhd::usrp::multi_usrp::sptr usrp)
+{
+    try {
+        std::string fixtype(
+                usrp->get_mboard_sensor("gps_fixtype", 0).to_pp_string());
+
+        if (fixtype.find("3d fix") == std::string::npos) {
+            etiLog.level(warn) << "OutputUHD: " << fixtype;
+
+            return false;
+        }
+
+        return true;
+    }
+    catch (uhd::lookup_error &e) {
+        etiLog.level(warn) << "OutputUHD: no gps_fixtype sensor";
+        return false;
+    }
+}
+
+
 OutputUHD::OutputUHD(
         const OutputUHDConfig& config) :
     ModOutput(ModFormat(1), ModFormat(0)),
@@ -68,13 +91,21 @@ OutputUHD::OutputUHD(
     // Since we don't know the buffer size, we cannot initialise
     // the buffers at object initialisation.
     first_run(true),
+    gps_fix_verified(false),
     activebuffer(1),
     myDelayBuf(0)
 
 {
-    myMuting = 0; // is remote-controllable
+    myMuting = true;     // is remote-controllable, and reset by the GPS fix check
     myStaticDelayUs = 0; // is remote-controllable
 
+    // Variables needed for GPS fix check
+    num_checks_without_gps_fix = 1;
+    first_gps_fix_check.tv_sec = 0;
+    last_gps_fix_check.tv_sec = 0;
+    time_last_frame.tv_sec = 0;
+
+
 #if FAKE_UHD
     MDEBUG("OutputUHD:Using fake UHD output");
 #else
@@ -104,8 +135,12 @@ OutputUHD::OutputUHD(
     RC_ADD_PARAMETER(muting, "Mute the output by stopping the transmitter");
     RC_ADD_PARAMETER(staticdelay, "Set static delay (uS) between 0 and 96000");
 
+    // TODO: find out how to use boost::bind to give the logger to the
+    // uhd_msg_handler
     uhd::msg::register_handler(uhd_msg_handler);
 
+    uhd::set_thread_priority_safe();
+
     //create a usrp device
     MDEBUG("OutputUHD:Creating the usrp device with: %s...\n",
             device.str().c_str());
@@ -163,59 +198,7 @@ OutputUHD::OutputUHD(
     MDEBUG("OutputUHD:Mute on missing timestamps: %s ...\n",
             myConf.muteNoTimestamps ? "enabled" : "disabled");
 
-    if (myConf.enableSync && (myConf.pps_src == "none")) {
-        etiLog.level(warn) <<
-            "OutputUHD: WARNING:"
-            " you are using synchronous transmission without PPS input!";
-
-        struct timespec now;
-        if (clock_gettime(CLOCK_REALTIME, &now)) {
-            perror("OutputUHD:Error: could not get time: ");
-            etiLog.level(error) << "OutputUHD: could not get time";
-        }
-        else {
-            myUsrp->set_time_now(uhd::time_spec_t(now.tv_sec));
-            etiLog.level(info) << "OutputUHD: Setting USRP time to " <<
-                    uhd::time_spec_t(now.tv_sec).get_real_secs();
-        }
-    }
-
-    if (myConf.pps_src != "none") {
-        /* handling time for synchronisation: wait until the next full
-         * second, and set the USRP time at next PPS */
-        struct timespec now;
-        time_t seconds;
-        if (clock_gettime(CLOCK_REALTIME, &now)) {
-            etiLog.level(error) << "OutputUHD: could not get time :" <<
-                strerror(errno);
-            throw std::runtime_error("OutputUHD: could not get time.");
-        }
-        else {
-            seconds = now.tv_sec;
-
-            MDEBUG("OutputUHD:sec+1: %ld ; now: %ld ...\n", seconds+1, now.tv_sec);
-            while (seconds + 1 > now.tv_sec) {
-                usleep(1);
-                if (clock_gettime(CLOCK_REALTIME, &now)) {
-                    etiLog.level(error) << "OutputUHD: could not get time :" <<
-                        strerror(errno);
-                    throw std::runtime_error("OutputUHD: could not get time.");
-                }
-            }
-            MDEBUG("OutputUHD:sec+1: %ld ; now: %ld ...\n", seconds+1, now.tv_sec);
-            /* We are now shortly after the second change. */
-
-            usleep(200000); // 200ms, we want the PPS to be later
-            myUsrp->set_time_unknown_pps(uhd::time_spec_t(seconds + 2));
-            etiLog.level(info) << "OutputUHD: Setting USRP time next pps to " <<
-                    uhd::time_spec_t(seconds + 2).get_real_secs();
-        }
-
-        usleep(1e6);
-        etiLog.log(info,  "OutputUHD: USRP time %f\n",
-                myUsrp->get_time_now().get_real_secs());
-    }
-
+    set_usrp_time();
 
     // preparing output thread worker data
     uwd.myUsrp = myUsrp;
@@ -241,9 +224,7 @@ OutputUHD::OutputUHD(
         uwd.check_gpsfix = false;
     }
 
-    uwd.max_gps_holdover = myConf.maxGPSHoldoverTime;
-
-    SetDelayBuffer(config.dabMode);
+    SetDelayBuffer(myConf.dabMode);
 
     shared_ptr<barrier> b(new barrier(2));
     mySyncBarrier = b;
@@ -263,28 +244,26 @@ OutputUHD::~OutputUHD()
     }
 }
 
-void OutputUHD::SetDelayBuffer(unsigned int dabMode)
+int transmission_frame_duration_ms(unsigned int dabMode)
 {
-    // find out the duration of the transmission frame (Table 2 in ETSI 300 401)
     switch (dabMode) {
-        case 0: // could happen when called from constructor and we take the mode from ETI
-            myTFDurationMs = 0;
-            break;
-        case 1:
-            myTFDurationMs = 96;
-            break;
-        case 2:
-            myTFDurationMs = 24;
-            break;
-        case 3:
-            myTFDurationMs = 24;
-            break;
-        case 4:
-            myTFDurationMs = 48;
-            break;
+        // could happen when called from constructor and we take the mode from ETI
+        case 0: return 0;
+
+        case 1: return 96;
+        case 2: return 24;
+        case 3: return 24;
+        case 4: return 48;
         default:
             throw std::runtime_error("OutputUHD: invalid DAB mode");
     }
+}
+
+void OutputUHD::SetDelayBuffer(unsigned int dabMode)
+{
+    // find out the duration of the transmission frame (Table 2 in ETSI 300 401)
+    myTFDurationMs = transmission_frame_duration_ms(dabMode);
+
     // The buffer size equals the number of samples per transmission frame so
     // we calculate it by multiplying the duration of the transmission frame
     // with the samplerate.
@@ -302,7 +281,22 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
     // the first buffer
     // We will only wait on the barrier on the subsequent calls to
     // OutputUHD::process
-    if (first_run) {
+    if (not gps_fix_verified) {
+        if (uwd.check_gpsfix) {
+            initial_gps_check();
+
+            if (num_checks_without_gps_fix == 0) {
+                set_usrp_time();
+                gps_fix_verified = true;
+                myMuting = false;
+            }
+        }
+        else {
+            gps_fix_verified = true;
+            myMuting = false;
+        }
+    }
+    else if (first_run) {
         etiLog.level(debug) << "OutputUHD: UHD initialising...";
 
         worker.start(&uwd);
@@ -350,6 +344,16 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
             throw std::runtime_error("Non-constant input length!");
         }
 
+        if (uwd.check_gpsfix) {
+            try {
+                check_gps();
+            }
+            catch (std::runtime_error& e) {
+                uwd.running = false;
+                etiLog.level(error) << e.what();
+            }
+        }
+
         mySyncBarrier.get()->wait();
 
         if (!uwd.running) {
@@ -404,114 +408,226 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
     }
 
     return uwd.bufsize;
-
 }
 
-void UHDWorker::process_errhandler()
+
+void OutputUHD::set_usrp_time()
 {
-    try {
-        process();
-    }
-    catch (fct_discontinuity_error& e) {
-        etiLog.level(warn) << e.what();
-        uwd->failed_due_to_fct = true;
+    if (myConf.enableSync && (myConf.pps_src == "none")) {
+        etiLog.level(warn) <<
+            "OutputUHD: WARNING:"
+            " you are using synchronous transmission without PPS input!";
+
+        struct timespec now;
+        if (clock_gettime(CLOCK_REALTIME, &now)) {
+            perror("OutputUHD:Error: could not get time: ");
+            etiLog.level(error) << "OutputUHD: could not get time";
+        }
+        else {
+            myUsrp->set_time_now(uhd::time_spec_t(now.tv_sec));
+            etiLog.level(info) << "OutputUHD: Setting USRP time to " <<
+                uhd::time_spec_t(now.tv_sec).get_real_secs();
+        }
     }
 
-    uwd->running = false;
-    uwd->sync_barrier.get()->wait();
-    etiLog.level(warn) << "UHD worker terminated";
+    if (myConf.pps_src != "none") {
+        /* handling time for synchronisation: wait until the next full
+         * second, and set the USRP time at next PPS */
+        struct timespec now;
+        time_t seconds;
+        if (clock_gettime(CLOCK_REALTIME, &now)) {
+            etiLog.level(error) << "OutputUHD: could not get time :" <<
+                strerror(errno);
+            throw std::runtime_error("OutputUHD: could not get time.");
+        }
+        else {
+            seconds = now.tv_sec;
+
+            MDEBUG("OutputUHD:sec+1: %ld ; now: %ld ...\n", seconds+1, now.tv_sec);
+            while (seconds + 1 > now.tv_sec) {
+                usleep(1);
+                if (clock_gettime(CLOCK_REALTIME, &now)) {
+                    etiLog.level(error) << "OutputUHD: could not get time :" <<
+                        strerror(errno);
+                    throw std::runtime_error("OutputUHD: could not get time.");
+                }
+            }
+            MDEBUG("OutputUHD:sec+1: %ld ; now: %ld ...\n", seconds+1, now.tv_sec);
+            /* We are now shortly after the second change. */
+
+            usleep(200000); // 200ms, we want the PPS to be later
+            myUsrp->set_time_unknown_pps(uhd::time_spec_t(seconds + 2));
+            etiLog.level(info) << "OutputUHD: Setting USRP time next pps to " <<
+                uhd::time_spec_t(seconds + 2).get_real_secs();
+        }
+
+        usleep(1e6);
+        etiLog.log(info,  "OutputUHD: USRP time %f\n",
+                myUsrp->get_time_now().get_real_secs());
+    }
 }
 
-// Check function for GPS fixtype
-bool check_gps_fix_ok(struct UHDWorkerData *uwd)
+void OutputUHD::initial_gps_check()
 {
-    try {
-        std::string fixtype(
-                uwd->myUsrp->get_mboard_sensor("gps_fixtype", 0).to_pp_string());
+    if (first_gps_fix_check.tv_sec == 0) {
+        etiLog.level(info) << "Waiting for GPS fix";
 
-        if (fixtype.find("3d fix") == std::string::npos) {
-            etiLog.level(warn) << "OutputUHD: " << fixtype;
+        if (clock_gettime(CLOCK_MONOTONIC, &first_gps_fix_check) != 0) {
+            stringstream ss;
+            ss << "clock_gettime failure: " << strerror(errno);
+            throw std::runtime_error(ss.str());
+        }
+    }
 
-            return false;
+    check_gps();
+
+    if (last_gps_fix_check.tv_sec >
+            first_gps_fix_check.tv_sec + initial_gps_fix_wait) {
+        stringstream ss;
+        ss << "GPS did not fix in " << initial_gps_fix_wait << " seconds";
+        throw std::runtime_error(ss.str());
+    }
+
+    if (time_last_frame.tv_sec == 0) {
+        if (clock_gettime(CLOCK_MONOTONIC, &time_last_frame) != 0) {
+            stringstream ss;
+            ss << "clock_gettime failure: " << strerror(errno);
+            throw std::runtime_error(ss.str());
         }
+    }
 
-        return true;
+    struct timespec now;
+    if (clock_gettime(CLOCK_MONOTONIC, &now) != 0) {
+        stringstream ss;
+        ss << "clock_gettime failure: " << strerror(errno);
+        throw std::runtime_error(ss.str());
     }
-    catch (uhd::lookup_error &e) {
-        etiLog.level(warn) << "OutputUHD: no gps_fixtype sensor";
-        return false;
+
+    long delta_us = timespecdiff_us(time_last_frame, now);
+    long wait_time_us = transmission_frame_duration_ms(myConf.dabMode);
+
+    if (wait_time_us - delta_us > 0) {
+        usleep(wait_time_us - delta_us);
+    }
+
+    time_last_frame.tv_nsec += wait_time_us * 1000;
+    if (time_last_frame.tv_nsec >= 1000000000L) {
+        time_last_frame.tv_nsec -= 1000000000L;
+        time_last_frame.tv_sec++;
     }
 }
 
-void UHDWorker::process()
+void OutputUHD::check_gps()
 {
-    int workerbuffer  = 0;
-    time_t tx_second = 0;
-    double pps_offset = 0;
-    double last_pps   = 2.0;
-    double usrp_time;
+    struct timespec time_now;
+    if (clock_gettime(CLOCK_MONOTONIC, &time_now) != 0) {
+        stringstream ss;
+        ss << "clock_gettime failure: " << strerror(errno);
+        throw std::runtime_error(ss.str());
+    }
 
-    // Variables needed for GPS fix check
-    double last_gps_fix_check = 0.0;
-    const double gps_fix_check_interval = 10.0; // seconds
-    int num_checks_without_gps_fix = 0;
-    boost::packaged_task<bool> gps_fix_pt;
-    boost::unique_future<bool> gps_fix_future;
-    boost::thread gps_fix_task;
+    // Divide interval by two because we alternate between
+    // launch and check
+    if (uwd.check_gpsfix and
+            last_gps_fix_check.tv_sec + gps_fix_check_interval/2.0 <
+            time_now.tv_sec) {
+        last_gps_fix_check = time_now;
 
-    // Asynchronous message statistics
-    int num_underflows = 0;
-    int num_late_packets = 0;
+        // Alternate between launching thread and checking the
+        // result.
+        if (gps_fix_task.joinable()) {
+            if (gps_fix_future.has_value()) {
 
-    //const struct timespec hundred_nano = {0, 100};
+                gps_fix_future.wait();
 
-    size_t sizeIn;
-    struct UHDWorkerFrameData* frame;
+                gps_fix_task.join();
 
-    size_t num_acc_samps; //number of accumulated samples
-    //int write_fail_count;
+                if (not gps_fix_future.get()) {
+                    if (num_checks_without_gps_fix == 0) {
+                        etiLog.level(alert) <<
+                            "OutputUHD: GPS Fix lost";
+                    }
+                    num_checks_without_gps_fix++;
+                }
+                else {
+                    if (num_checks_without_gps_fix) {
+                        etiLog.level(info) <<
+                            "OutputUHD: GPS Fix recovered";
+                    }
+                    num_checks_without_gps_fix = 0;
+                }
 
-    // Transmit timeout
-    const double timeout = 20.0;
+                if (gps_fix_check_interval * num_checks_without_gps_fix >
+                        myConf.maxGPSHoldoverTime) {
+                    std::stringstream ss;
+                    ss << "Lost GPS fix for " << gps_fix_check_interval *
+                        num_checks_without_gps_fix << " seconds";
+                    throw std::runtime_error(ss.str());
+                }
+            }
+        }
+        else {
+            // Checking the sensor here takes too much
+            // time, it has to be done in a separate thread.
+            gps_fix_pt = boost::packaged_task<bool>(
+                    boost::bind(check_gps_fix_ok, myUsrp) );
+
+            gps_fix_future = gps_fix_pt.get_future();
+
+            gps_fix_task = boost::thread(boost::move(gps_fix_pt));
+        }
+    }
+}
+
+//============================ UHD Worker ========================
 
-    // Set thread priority to realtime
-    const int policy = SCHED_RR;
-    sched_param sp;
-    sp.sched_priority = sched_get_priority_min(policy);
-    int ret = pthread_setschedparam(pthread_self(), policy, &sp);
-    if (ret != 0) {
-        etiLog.level(error) << "Could not set priority for UHD thread:" << ret;
+void UHDWorker::process_errhandler()
+{
+    try {
+        process();
+    }
+    catch (fct_discontinuity_error& e) {
+        etiLog.level(warn) << e.what();
+        uwd->failed_due_to_fct = true;
     }
 
+    uwd->running = false;
+    uwd->sync_barrier.get()->wait();
+    etiLog.level(warn) << "UHD worker terminated";
+}
+
+void UHDWorker::process()
+{
+    int workerbuffer = 0;
+    tx_second        = 0;
+    pps_offset       = 0.0;
+    last_pps         = 2.0;
 
 #if FAKE_UHD == 0
     uhd::stream_args_t stream_args("fc32"); //complex floats
-    uhd::tx_streamer::sptr myTxStream = uwd->myUsrp->get_tx_stream(stream_args);
-    size_t usrp_max_num_samps = myTxStream->get_max_num_samps();
-#else
-    size_t usrp_max_num_samps = 2048; // arbitrarily chosen
+    myTxStream = uwd->myUsrp->get_tx_stream(stream_args);
 #endif
 
-    const complexf* in;
-
-    uhd::tx_metadata_t md;
     md.start_of_burst = false;
-    md.end_of_burst = false;
+    md.end_of_burst   = false;
+
+    expected_next_fct = -1;
 
-    int expected_next_fct = -1;
+    num_underflows   = 0;
+    num_late_packets = 0;
 
     while (uwd->running) {
-        bool fct_discontinuity = false;
-        md.has_time_spec = false;
-        md.time_spec = uhd::time_spec_t(0.0);
-        num_acc_samps = 0;
-        //write_fail_count = 0;
+        fct_discontinuity = false;
+        md.has_time_spec  = false;
+        md.time_spec      = uhd::time_spec_t(0.0);
 
         /* Wait for barrier */
         // this wait will hopefully always be the second one
         // because modulation should be quicker than transmission
         uwd->sync_barrier.get()->wait();
 
+        struct UHDWorkerFrameData* frame;
+
         if (workerbuffer == 0) {
             frame = &(uwd->frame0);
         }
@@ -523,308 +639,240 @@ void UHDWorker::process()
                     "UHDWorker.process: workerbuffer is neither 0 nor 1 !");
         }
 
-        in = reinterpret_cast<const complexf*>(frame->buf);
-        pps_offset = frame->ts.timestamp_pps_offset;
+        handle_frame(frame);
 
-        // Tx second from MNSC
-        tx_second = frame->ts.timestamp_sec;
+        // swap buffers
+        workerbuffer = (workerbuffer + 1) % 2;
+    }
+}
 
-        sizeIn = uwd->bufsize / sizeof(complexf);
+void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame)
+{
+    // Transmit timeout
+    static const double tx_timeout = 20.0;
 
-        /* Verify that the FCT value is correct. If we miss one transmission
-         * frame we must interrupt UHD and resync to the timestamps
-         */
-        if (frame->ts.fct == -1) {
-            etiLog.level(info) <<
-                "OutputUHD: dropping one frame with invalid FCT";
-            goto loopend;
-        }
-        if (expected_next_fct != -1) {
-            if (expected_next_fct != (int)frame->ts.fct) {
-                etiLog.level(warn) <<
-                    "OutputUHD: Incorrect expect fct " << frame->ts.fct <<
-                    ", expected " << expected_next_fct;
+    pps_offset = frame->ts.timestamp_pps_offset;
 
-                fct_discontinuity = true;
-                throw fct_discontinuity_error();
-            }
-        }
+    // Tx second from MNSC
+    tx_second = frame->ts.timestamp_sec;
 
-        expected_next_fct = (frame->ts.fct + uwd->fct_increment) % 250;
+    /* Verify that the FCT value is correct. If we miss one transmission
+     * frame we must interrupt UHD and resync to the timestamps
+     */
+    if (frame->ts.fct == -1) {
+        etiLog.level(info) <<
+            "OutputUHD: dropping one frame with invalid FCT";
+        return;
+    }
+    if (expected_next_fct != -1) {
+        if (expected_next_fct != (int)frame->ts.fct) {
+            etiLog.level(warn) <<
+                "OutputUHD: Incorrect expect fct " << frame->ts.fct <<
+                ", expected " << expected_next_fct;
 
-        // Check for ref_lock
-        if (uwd->check_refclk_loss) {
-            try {
-                // TODO: Is this check specific to the B100 and USRP2 ?
-                if (! uwd->myUsrp->get_mboard_sensor("ref_locked", 0).to_bool()) {
-                    etiLog.log(alert,
-                            "OutputUHD: External reference clock lock lost !");
-                    if (uwd->refclk_lock_loss_behaviour == CRASH) {
-                        throw std::runtime_error(
-                                "OutputUHD: External reference clock lock lost.");
-                    }
-                }
-            }
-            catch (uhd::lookup_error &e) {
-                uwd->check_refclk_loss = false;
-                etiLog.log(warn,
-                        "OutputUHD: This USRP does not have mboard sensor for ext clock loss."
-                        " Check disabled.");
-            }
+            fct_discontinuity = true;
+            throw fct_discontinuity_error();
         }
+    }
 
-        usrp_time = uwd->myUsrp->get_time_now().get_real_secs();
-
-        if (uwd->check_gpsfix and
-                // Divide interval by two because we alternate between
-                // launch and check
-                last_gps_fix_check + gps_fix_check_interval/2.0 < usrp_time) {
-            last_gps_fix_check = usrp_time;
-
-            // Alternate between launching thread and checking the
-            // result.
-            if (gps_fix_task.joinable()) {
-                if (gps_fix_future.has_value()) {
-
-                    gps_fix_future.wait();
-
-                    gps_fix_task.join();
+    expected_next_fct = (frame->ts.fct + uwd->fct_increment) % 250;
 
-                    if (not gps_fix_future.get()) {
-                        if (not num_checks_without_gps_fix) {
-                            etiLog.level(alert) <<
-                                "OutputUHD: GPS Fix lost";
-                        }
-                        num_checks_without_gps_fix++;
-                    }
-                    else {
-                        if (num_checks_without_gps_fix) {
-                            etiLog.level(info) <<
-                                "OutputUHD: GPS Fix recovered";
-                        }
-                        num_checks_without_gps_fix = 0;
-                    }
-
-                    if (gps_fix_check_interval * num_checks_without_gps_fix >
-                            uwd->max_gps_holdover) {
-                        std::stringstream ss;
-                        ss << "Lost GPS fix for " << gps_fix_check_interval *
-                            num_checks_without_gps_fix << " seconds";
-                        throw std::runtime_error(ss.str());
-                    }
+    // Check for ref_lock
+    if (uwd->check_refclk_loss) {
+        try {
+            // TODO: Is this check specific to the B100 and USRP2 ?
+            if (! uwd->myUsrp->get_mboard_sensor("ref_locked", 0).to_bool()) {
+                etiLog.log(alert,
+                        "OutputUHD: External reference clock lock lost !");
+                if (uwd->refclk_lock_loss_behaviour == CRASH) {
+                    throw std::runtime_error(
+                            "OutputUHD: External reference clock lock lost.");
                 }
             }
-            else {
-                // Checking the sensor here takes too much
-                // time, it has to be done in a separate thread.
-                gps_fix_pt = boost::packaged_task<bool>(
-                        boost::bind(check_gps_fix_ok, uwd) );
-
-                gps_fix_future = gps_fix_pt.get_future();
-
-                gps_fix_task = boost::thread(boost::move(gps_fix_pt));
-            }
         }
+        catch (uhd::lookup_error &e) {
+            uwd->check_refclk_loss = false;
+            etiLog.log(warn,
+                    "OutputUHD: This USRP does not have mboard sensor for ext clock loss."
+                    " Check disabled.");
+        }
+    }
 
+    double usrp_time = uwd->myUsrp->get_time_now().get_real_secs();
 
-        if (uwd->sourceContainsTimestamp) {
-            if (!frame->ts.timestamp_valid) {
-                /* We have not received a full timestamp through
-                 * MNSC. We sleep through the frame.
-                 */
-                etiLog.level(info) <<
-                    "OutputUHD: Throwing sample " << frame->ts.fct <<
-                    " away: incomplete timestamp " << tx_second <<
-                    " + " << pps_offset;
-                usleep(20000); //TODO should this be TM-dependant ?
-                goto loopend;
-            }
 
-            md.has_time_spec = true;
-            md.time_spec = uhd::time_spec_t(tx_second, pps_offset);
-
-            // md is defined, let's do some checks
-            if (md.time_spec.get_real_secs() + timeout < usrp_time) {
-                etiLog.level(warn) <<
-                    "OutputUHD: Timestamp in the past! offset: " <<
-                    md.time_spec.get_real_secs() - usrp_time <<
-                    "  (" << usrp_time << ")"
-                    " frame " << frame->ts.fct <<
-                    ", tx_second " << tx_second <<
-                    ", pps " << pps_offset;
-                goto loopend; //skip the frame
-            }
+    if (uwd->sourceContainsTimestamp) {
+        if (!frame->ts.timestamp_valid) {
+            /* We have not received a full timestamp through
+             * MNSC. We sleep through the frame.
+             */
+            etiLog.level(info) <<
+                "OutputUHD: Throwing sample " << frame->ts.fct <<
+                " away: incomplete timestamp " << tx_second <<
+                " + " << pps_offset;
+            usleep(20000); //TODO should this be TM-dependant ?
+            return;
+        }
 
-#if 0 // Let uhd handle this
-            if (md.time_spec.get_real_secs() > usrp_time + TIMESTAMP_MARGIN_FUTURE) {
-                etiLog.level(warn) <<
-                        "OutputUHD: Timestamp too far in the future! offset: " <<
-                        md.time_spec.get_real_secs() - usrp_time;
-                usleep(20000); //sleep so as to fill buffers
-            }
-#endif
+        md.has_time_spec = true;
+        md.time_spec = uhd::time_spec_t(tx_second, pps_offset);
+
+        // md is defined, let's do some checks
+        if (md.time_spec.get_real_secs() + tx_timeout < usrp_time) {
+            etiLog.level(warn) <<
+                "OutputUHD: Timestamp in the past! offset: " <<
+                md.time_spec.get_real_secs() - usrp_time <<
+                "  (" << usrp_time << ")"
+                " frame " << frame->ts.fct <<
+                ", tx_second " << tx_second <<
+                ", pps " << pps_offset;
+            return;
+        }
 
-            if (md.time_spec.get_real_secs() > usrp_time + TIMESTAMP_ABORT_FUTURE) {
-                etiLog.level(error) <<
-                        "OutputUHD: Timestamp way too far in the future! offset: " <<
-                        md.time_spec.get_real_secs() - usrp_time;
-                throw std::runtime_error("Timestamp error. Aborted.");
-            }
+        if (md.time_spec.get_real_secs() > usrp_time + TIMESTAMP_ABORT_FUTURE) {
+            etiLog.level(error) <<
+                "OutputUHD: Timestamp way too far in the future! offset: " <<
+                md.time_spec.get_real_secs() - usrp_time;
+            throw std::runtime_error("Timestamp error. Aborted.");
         }
-        else { // !uwd->sourceContainsTimestamp
-            if (uwd->muting || uwd->muteNoTimestamps) {
-                /* There was some error decoding the timestamp
-                */
-                if (uwd->muting) {
-                    etiLog.log(info,
-                            "OutputUHD: Muting sample %d requested\n",
-                            frame->ts.fct);
-                }
-                else {
-                    etiLog.log(info,
-                            "OutputUHD: Muting sample %d : no timestamp\n",
-                            frame->ts.fct);
-                }
-                usleep(20000);
-                goto loopend;
+    }
+    else { // !uwd->sourceContainsTimestamp
+        if (uwd->muting || uwd->muteNoTimestamps) {
+            /* There was some error decoding the timestamp
+            */
+            if (uwd->muting) {
+                etiLog.log(info,
+                        "OutputUHD: Muting sample %d requested\n",
+                        frame->ts.fct);
             }
+            else {
+                etiLog.log(info,
+                        "OutputUHD: Muting sample %d : no timestamp\n",
+                        frame->ts.fct);
+            }
+            usleep(20000);
+            return;
         }
+    }
 
-        PDEBUG("UHDWorker::process:max_num_samps: %zu.\n",
-                usrp_max_num_samps);
+    tx_frame(frame);
 
-        while (uwd->running && !uwd->muting && (num_acc_samps < sizeIn)) {
-            size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);
+    if (last_pps > pps_offset) {
+        if (num_underflows or num_late_packets) {
+            etiLog.log(info,
+                    "OutputUHD status (usrp time: %f): "
+                    "%d underruns and %d late packets since last status.\n",
+                    usrp_time,
+                    num_underflows, num_late_packets);
+        }
+        num_underflows = 0;
+        num_late_packets = 0;
+    }
 
-            //ensure the the last packet has EOB set if the timestamps has been
-            //refreshed and need to be reconsidered.
-            //Also, if we saw that the FCT did not increment as expected, which
-            //could be due to a lost incoming packet.
-            md.end_of_burst = (
-                    uwd->sourceContainsTimestamp &&
-                    (frame->ts.timestamp_refresh || fct_discontinuity) &&
-                    samps_to_send <= usrp_max_num_samps );
+    last_pps = pps_offset;
+}
 
+void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame)
+{
+    const double tx_timeout = 20.0;
+    const size_t sizeIn = uwd->bufsize / sizeof(complexf);
+    const complexf* in_data = reinterpret_cast<const complexf*>(frame->buf);
 
-#if FAKE_UHD
-            // This is probably very approximate
-            usleep( (1000000 / uwd->sampleRate) * samps_to_send);
-            size_t num_tx_samps = samps_to_send;
+#if FAKE_UHD == 0
+    size_t usrp_max_num_samps = myTxStream->get_max_num_samps();
 #else
-            //send a single packet
-            size_t num_tx_samps = myTxStream->send(
-                    &in[num_acc_samps],
-                    samps_to_send, md, timeout);
+    size_t usrp_max_num_samps = 2048; // arbitrarily chosen
 #endif
 
-            num_acc_samps += num_tx_samps;
+    size_t num_acc_samps = 0; //number of accumulated samples
+    while (uwd->running && !uwd->muting && (num_acc_samps < sizeIn)) {
+        size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);
 
-            md.time_spec = uhd::time_spec_t(tx_second, pps_offset)
-                + uhd::time_spec_t(0, num_acc_samps/uwd->sampleRate);
+        //ensure the the last packet has EOB set if the timestamps has been
+        //refreshed and need to be reconsidered.
+        //Also, if we saw that the FCT did not increment as expected, which
+        //could be due to a lost incoming packet.
+        md.end_of_burst = (
+                uwd->sourceContainsTimestamp &&
+                (frame->ts.timestamp_refresh || fct_discontinuity) &&
+                samps_to_send <= usrp_max_num_samps );
 
-            /*
-               fprintf(stderr, "*** pps_offset %f, md.time_spec %f, usrp->now %f\n",
-               pps_offset,
-               md.time_spec.get_real_secs(),
-               uwd->myUsrp->get_time_now().get_real_secs());
-            // */
 
-
-            if (num_tx_samps == 0) {
-#if 1
-                etiLog.log(warn,
-                        "UHDWorker::process() unable to write to device, skipping frame!\n");
-                break;
+#if FAKE_UHD
+        // This is probably very approximate
+        usleep( (1000000 / uwd->sampleRate) * samps_to_send);
+        size_t num_tx_samps = samps_to_send;
 #else
-                // This has been disabled, because if there is a write failure,
-                // we'd better not insist and try to go on transmitting future
-                // frames.
-                // The goal is not to try to send by all means possible. It's
-                // more important to make sure the SFN is not disturbed.
-
-                fprintf(stderr, "F");
-                nanosleep(&hundred_nano, NULL);
-                write_fail_count++;
-                if (write_fail_count >= 3) {
-                    double ts = md.time_spec.get_real_secs();
-                    double t_usrp = uwd->myUsrp->get_time_now().get_real_secs();
-
-                    fprintf(stderr, "*** USRP write fail count %d\n", write_fail_count);
-                    fprintf(stderr, "*** delta %f, md.time_spec %f, usrp->now %f\n",
-                            ts - t_usrp,
-                            ts, t_usrp);
-
-                    fprintf(stderr, "UHDWorker::process() unable to write to device, skipping frame!\n");
-                    break;
-                }
+        //send a single packet
+        size_t num_tx_samps = myTxStream->send(
+                &in_data[num_acc_samps],
+                samps_to_send, md, tx_timeout);
 #endif
-            }
 
-#if FAKE_UHD == 0
-            uhd::async_metadata_t async_md;
-            if (uwd->myUsrp->get_device()->recv_async_msg(async_md, 0)) {
-                const char* uhd_async_message = "";
-                bool failure = false;
-                switch (async_md.event_code) {
-                    case uhd::async_metadata_t::EVENT_CODE_BURST_ACK:
-                        break;
-                    case uhd::async_metadata_t::EVENT_CODE_UNDERFLOW:
-                        uhd_async_message = "Underflow";
-                        num_underflows++;
-                        break;
-                    case uhd::async_metadata_t::EVENT_CODE_SEQ_ERROR:
-                        uhd_async_message = "Packet loss between host and device.";
-                        failure = true;
-                        break;
-                    case uhd::async_metadata_t::EVENT_CODE_TIME_ERROR:
-                        uhd_async_message = "Packet had time that was late.";
-                        num_late_packets++;
-                        break;
-                    case uhd::async_metadata_t::EVENT_CODE_UNDERFLOW_IN_PACKET:
-                        uhd_async_message = "Underflow occurred inside a packet.";
-                        failure = true;
-                        break;
-                    case uhd::async_metadata_t::EVENT_CODE_SEQ_ERROR_IN_BURST:
-                        uhd_async_message = "Packet loss within a burst.";
-                        failure = true;
-                        break;
-                    default:
-                        uhd_async_message = "unknown event code";
-                        failure = true;
-                        break;
-                }
+        num_acc_samps += num_tx_samps;
 
-                if (failure) {
-                    etiLog.level(alert) << "Near frame " <<
-                        frame->ts.fct << ": Received Async UHD Message '" << 
-                        uhd_async_message << "'";
+        md.time_spec = uhd::time_spec_t(tx_second, pps_offset)
+            + uhd::time_spec_t(0, num_acc_samps/uwd->sampleRate);
 
-                }
-            }
-#endif
+        if (num_tx_samps == 0) {
+            etiLog.log(warn,
+                    "UHDWorker::process() unable to write to device, skipping frame!\n");
+            break;
         }
 
-        if (last_pps > pps_offset) {
-            if (num_underflows or num_late_packets) {
-                etiLog.log(info,
-                        "OutputUHD status (usrp time: %f): "
-                        "%d underruns and %d late packets since last status.\n",
-                        usrp_time,
-                        num_underflows, num_late_packets);
-            }
-            num_underflows = 0;
-            num_late_packets = 0;
-        }
+        print_async_metadata(frame);
+    }
+}
 
+void UHDWorker::print_async_metadata(const struct UHDWorkerFrameData *frame)
+{
+#if FAKE_UHD == 0
+    uhd::async_metadata_t async_md;
+    if (uwd->myUsrp->get_device()->recv_async_msg(async_md, 0)) {
+        const char* uhd_async_message = "";
+        bool failure = false;
+        switch (async_md.event_code) {
+            case uhd::async_metadata_t::EVENT_CODE_BURST_ACK:
+                break;
+            case uhd::async_metadata_t::EVENT_CODE_UNDERFLOW:
+                uhd_async_message = "Underflow";
+                num_underflows++;
+                break;
+            case uhd::async_metadata_t::EVENT_CODE_SEQ_ERROR:
+                uhd_async_message = "Packet loss between host and device.";
+                failure = true;
+                break;
+            case uhd::async_metadata_t::EVENT_CODE_TIME_ERROR:
+                uhd_async_message = "Packet had time that was late.";
+                num_late_packets++;
+                break;
+            case uhd::async_metadata_t::EVENT_CODE_UNDERFLOW_IN_PACKET:
+                uhd_async_message = "Underflow occurred inside a packet.";
+                failure = true;
+                break;
+            case uhd::async_metadata_t::EVENT_CODE_SEQ_ERROR_IN_BURST:
+                uhd_async_message = "Packet loss within a burst.";
+                failure = true;
+                break;
+            default:
+                uhd_async_message = "unknown event code";
+                failure = true;
+                break;
+        }
 
-        last_pps = pps_offset;
+        if (failure) {
+            etiLog.level(alert) << "Near frame " <<
+                frame->ts.fct << ": Received Async UHD Message '" << 
+                uhd_async_message << "'";
 
-loopend:
-        // swap buffers
-        workerbuffer = (workerbuffer + 1) % 2;
+        }
     }
+#endif
 }
 
+// =======================================
+// Remote Control for UHD
+// =======================================
 
 void OutputUHD::set_parameter(const string& parameter, const string& value)
 {
diff --git a/src/OutputUHD.h b/src/OutputUHD.h
index 44dd0ff..633de04 100644
--- a/src/OutputUHD.h
+++ b/src/OutputUHD.h
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2014
+   Copyright (C) 2014, 2015
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -50,6 +50,7 @@ DESCRIPTION:
 #include <boost/thread/thread.hpp>
 #include <boost/thread/barrier.hpp>
 #include <boost/shared_ptr.hpp>
+#include <boost/atomic.hpp>
 #include <list>
 #include <string>
 
@@ -94,7 +95,7 @@ struct fct_discontinuity_error : public std::exception
 enum refclk_lock_loss_behaviour_t { CRASH, IGNORE };
 
 struct UHDWorkerData {
-    bool running;
+    boost::atomic<bool> running;
     bool failed_due_to_fct;
 
 #if FAKE_UHD == 0
@@ -123,10 +124,6 @@ struct UHDWorkerData {
     // If we want to check for the gps_fixtype sensor
     bool check_gpsfix;
 
-    // After how much time without fix we abort
-    int max_gps_holdover; // seconds
-
-
     // muting set by remote control
     bool muting;
 
@@ -160,14 +157,29 @@ class UHDWorker {
         }
 
     private:
-        void process();
-        void process_errhandler();
+        // Asynchronous message statistics
+        int num_underflows;
+        int num_late_packets;
 
+        bool fct_discontinuity;
+        int expected_next_fct;
+        uhd::tx_metadata_t md;
+        time_t tx_second;
+        double pps_offset;
+        double last_pps;
+
+        void print_async_metadata(const struct UHDWorkerFrameData *frame);
+
+        void handle_frame(const struct UHDWorkerFrameData *frame);
+        void tx_frame(const struct UHDWorkerFrameData *frame);
 
         struct UHDWorkerData *uwd;
         boost::thread uhd_thread;
 
         uhd::tx_streamer::sptr myTxStream;
+
+        void process();
+        void process_errhandler();
 };
 
 /* This structure is used as initial configuration for OutputUHD */
@@ -239,6 +251,7 @@ class OutputUHD: public ModOutput, public RemoteControllable {
         boost::shared_ptr<boost::barrier> mySyncBarrier;
         UHDWorker worker;
         bool first_run;
+        bool gps_fix_verified;
         struct UHDWorkerData uwd;
         int activebuffer;
 
@@ -255,6 +268,27 @@ class OutputUHD: public ModOutput, public RemoteControllable {
         int myTFDurationMs; // TF duration in milliseconds
         std::vector<complexf> myDelayBuf;
         size_t lastLen;
+
+        // GPS Fix check variables
+        int num_checks_without_gps_fix;
+        struct timespec first_gps_fix_check;
+        struct timespec last_gps_fix_check;
+        struct timespec time_last_frame;
+        boost::packaged_task<bool> gps_fix_pt;
+        boost::unique_future<bool> gps_fix_future;
+        boost::thread gps_fix_task;
+
+        // Wait time in seconds to get fix
+        static const int initial_gps_fix_wait = 60;
+
+        // Interval for checking the GPS at runtime
+        static const double gps_fix_check_interval = 10.0; // seconds
+
+        void check_gps();
+
+        void set_usrp_time();
+
+        void initial_gps_check();
 };
 
 #endif // HAVE_OUTPUT_UHD
diff --git a/src/RemoteControl.cpp b/src/RemoteControl.cpp
index 666f891..21a6c81 100644
--- a/src/RemoteControl.cpp
+++ b/src/RemoteControl.cpp
@@ -310,7 +310,7 @@ void RemoteControllerZmq::process()
 {
     // create zmq reply socket for receiving ctrl parameters
     zmq::socket_t repSocket(m_zmqContext, ZMQ_REP);
-    std::cout << "Starting zmq remote control thread" << std::endl;
+    std::cerr << "Starting zmq remote control thread" << std::endl;
     try
     {
         // connect the socket
@@ -344,10 +344,9 @@ void RemoteControllerZmq::process()
                     try
                     {
                         std::string value = get_param_(module, parameter);
-                        zmq::message_t *pMsg = new zmq::message_t(value.size());
-                        memcpy ((void*) pMsg->data(), value.data(), value.size());
-                        repSocket.send(*pMsg, 0);
-                        delete pMsg;
+                        zmq::message_t msg(value.size());
+                        memcpy ((void*) msg.data(), value.data(), value.size());
+                        repSocket.send(&msg, 0);
                     }
                     catch (ParameterError &err)
                     {
diff --git a/src/RemoteControl.h b/src/RemoteControl.h
index 8f4bdaf..1b5e447 100644
--- a/src/RemoteControl.h
+++ b/src/RemoteControl.h
@@ -34,7 +34,7 @@
 #endif
 
 #if defined(HAVE_ZEROMQ)
-#include <zmq.hpp>
+#include "zmq.hpp"
 #endif
 
 #include <list>
diff --git a/src/TimestampDecoder.cpp b/src/TimestampDecoder.cpp
index c833e7a..5044366 100644
--- a/src/TimestampDecoder.cpp
+++ b/src/TimestampDecoder.cpp
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2014
+   Copyright (C) 2014, 2015
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -29,6 +29,7 @@
 #include <fstream>
 #include <string>
 #include <boost/lexical_cast.hpp>
+#include <boost/make_shared.hpp>
 #include <sys/types.h>
 #include "PcDebug.h"
 #include "TimestampDecoder.h"
@@ -41,7 +42,8 @@
 
 void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
 {
-    struct frame_timestamp* ts_queued = new struct frame_timestamp;
+    boost::shared_ptr<struct frame_timestamp> ts_queued =
+        boost::make_shared<struct frame_timestamp>();
 
     /* Push new timestamp into queue */
     ts_queued->timestamp_valid = full_timestamp_received_mnsc;
@@ -62,8 +64,8 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
      *
      * Therefore, use <= and not < for comparison
      */
-    if (queue_timestamps.size() <= modconfig.delay_calculation_pipeline_stages) {
-        //fprintf(stderr, "* %zu %u ", queue_timestamps.size(), modconfig.delay_calculation_pipeline_stages);
+    if (queue_timestamps.size() <= m_tist_delay_stages) {
+        //fprintf(stderr, "* %zu %u ", queue_timestamps.size(), m_tist_delay_stages);
         /* Return invalid timestamp until the queue is full */
         ts.timestamp_valid = false;
         ts.timestamp_sec = 0;
@@ -87,15 +89,13 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
                 ts.timestamp_sec,
                 ts.timestamp_pps_offset,
                 ts.timestamp_refresh);*/
-
-        delete ts_queued;
     }
 
     MDEBUG("Timestamp queue size %zu, delay_calc %u\n",
             queue_timestamps.size(),
-            modconfig.delay_calculation_pipeline_stages);
+            m_tist_delay_stages);
 
-    if (queue_timestamps.size() > modconfig.delay_calculation_pipeline_stages) {
+    if (queue_timestamps.size() > m_tist_delay_stages) {
         etiLog.level(error) << "Error: Timestamp queue is too large : size " <<
             queue_timestamps.size() << "! This should not happen !";
     }
@@ -198,77 +198,41 @@ void TimestampDecoder::updateTimestampEti(
     latestFCT = fct;
 }
 
-
-bool TimestampDecoder::updateModulatorOffset()
+void TimestampDecoder::set_parameter(
+        const std::string& parameter,
+        const std::string& value)
 {
     using namespace std;
-    using boost::lexical_cast;
-    using boost::bad_lexical_cast;
 
-    if (modconfig.use_offset_fixed)
-    {
-        timestamp_offset = modconfig.offset_fixed;
-        return true;
-    }
-    else if (modconfig.use_offset_file)
-    {
-        bool r = false;
-        double newoffset;
+    stringstream ss(value);
+    ss.exceptions ( stringstream::failbit | stringstream::badbit );
 
-        std::string filedata;
-        ifstream filestream;
-
-        try
-        {
-            filestream.open(modconfig.offset_filename.c_str());
-            if (!filestream.eof())
-            {
-                getline(filestream, filedata);
-                try
-                {
-                    newoffset = lexical_cast<double>(filedata);
-                    r = true;
-                }
-                catch (bad_lexical_cast& e)
-                {
-                    etiLog.level(error) <<
-                        "Error parsing timestamp offset from file '" <<
-                        modconfig.offset_filename << "'";
-                    r = false;
-                }
-            }
-            else
-            {
-                etiLog.level(error) <<
-                    "Error reading from timestamp offset file: eof reached\n";
-                r = false;
-            }
-            filestream.close();
-        }
-        catch (exception& e)
-        {
-            etiLog.level(error) << "Error opening timestamp offset file\n";
-            r = false;
-        }
-
-
-        if (r)
-        {
-            if (timestamp_offset != newoffset)
-            {
-                timestamp_offset = newoffset;
-                etiLog.level(info) <<
-                    "TimestampDecoder::updateTimestampOffset: new offset is " <<
-                    timestamp_offset;
-                offset_changed = true;
-            }
+    if (parameter == "offset") {
+        ss >> timestamp_offset;
+        offset_changed = true;
+    }
+    else {
+        stringstream ss;
+        ss << "Parameter '" << parameter
+            << "' is not exported by controllable " << get_rc_name();
+        throw ParameterError(ss.str());
+    }
+}
 
-        }
+const std::string TimestampDecoder::get_parameter(
+        const std::string& parameter) const
+{
+    using namespace std;
 
-        return r;
+    stringstream ss;
+    if (parameter == "offset") {
+        ss << timestamp_offset;
     }
     else {
-        return false;
+        ss << "Parameter '" << parameter <<
+            "' is not exported by controllable " << get_rc_name();
+        throw ParameterError(ss.str());
     }
+    return ss.str();
 }
 
diff --git a/src/TimestampDecoder.h b/src/TimestampDecoder.h
index 82753d1..d8ab633 100644
--- a/src/TimestampDecoder.h
+++ b/src/TimestampDecoder.h
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2014
+   Copyright (C) 2014, 2015
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -28,29 +28,14 @@
 #define TIMESTAMP_DECODER_H
 
 #include <queue>
+#include <boost/shared_ptr.hpp>
 #include <string>
 #include <time.h>
 #include <math.h>
 #include <stdio.h>
 #include "Eti.h"
 #include "Log.h"
-
-struct modulator_offset_config
-{
-    bool use_offset_fixed;
-    double offset_fixed;
-    /* These two fields are used when the modulator is run with a fixed offset */
-
-    bool use_offset_file;
-    std::string offset_filename;
-    /* These two fields are used when the modulator reads the offset from a file */
-
-    unsigned delay_calculation_pipeline_stages;
-    /* Specifies by how many stages the timestamp must be delayed.
-     * (e.g. The FIRFilter is pipelined, therefore we must increase 
-     * delay_calculation_pipeline_stages by one if the filter is used
-     */
-};
+#include "RemoteControl.h"
 
 struct frame_timestamp
 {
@@ -109,13 +94,24 @@ struct frame_timestamp
 };
 
 /* This module decodes MNSC time information */
-class TimestampDecoder
+class TimestampDecoder : public RemoteControllable
 {
     public:
         TimestampDecoder(
-                struct modulator_offset_config& config) :
-            modconfig(config)
+                /* The modulator adds this offset to the TIST to define time of
+                 * frame transmission
+                 */
+                double offset_s,
+
+                /* Specifies by how many stages the timestamp must be delayed.
+                 * (e.g. The FIRFilter is pipelined, therefore we must increase
+                 * tist_delay_stages by one if the filter is used
+                 */
+                unsigned tist_delay_stages) :
+                RemoteControllable("tist")
         {
+            timestamp_offset = offset_s;
+            m_tist_delay_stages = tist_delay_stages;
             inhibit_second_update = 0;
             time_pps = 0.0;
             time_secs = 0;
@@ -125,10 +121,10 @@ class TimestampDecoder
             gmtime_r(0, &temp_time);
             offset_changed = false;
 
+            RC_ADD_PARAMETER(offset, "TIST offset [s]");
+
             etiLog.level(info) << "Setting up timestamp decoder with " <<
-                (modconfig.use_offset_fixed ? "fixed" :
-                (modconfig.use_offset_file ? "dynamic" : "none")) <<
-                " offset";
+                timestamp_offset << " offset";
 
         };
 
@@ -142,9 +138,21 @@ class TimestampDecoder
                 double pps,
                 int32_t fct);
 
-        /* Update the modulator timestamp offset according to the modconf
+        /*********** REMOTE CONTROL ***************/
+        /* virtual void enrol_at(BaseRemoteController& controller)
+         * is inherited
          */
-        bool updateModulatorOffset();
+
+        /* Base function to set parameters. */
+        virtual void set_parameter(const std::string& parameter,
+                const std::string& value);
+
+        /* Getting a parameter always returns a string. */
+        virtual const std::string get_parameter(
+                const std::string& parameter) const;
+
+        const char* name() { return "TS"; }
+
 
     protected:
         /* Push a new MNSC field into the decoder */
@@ -167,12 +175,10 @@ class TimestampDecoder
         int32_t latestFCT;
         double time_pps;
         double timestamp_offset;
+        unsigned m_tist_delay_stages;
         int inhibit_second_update;
         bool offset_changed;
 
-        /* configuration for the offset management */
-        struct modulator_offset_config& modconfig;
-
         /* When the type or identifier don't match, the decoder must
          * be disabled
          */
@@ -186,8 +192,9 @@ class TimestampDecoder
          * synchronise two modulators if only one uses (for instance) the
          * FIRFilter (1 stage pipeline)
          */
-        std::queue<struct frame_timestamp*> queue_timestamps;
+        std::queue<boost::shared_ptr<struct frame_timestamp> > queue_timestamps;
 
 };
 
 #endif
+
diff --git a/src/Utils.cpp b/src/Utils.cpp
index 8b97602..6c9b0fc 100644
--- a/src/Utils.cpp
+++ b/src/Utils.cpp
@@ -49,7 +49,6 @@ void printUsage(char* progName)
             " (-f filename | -u uhddevice -F frequency) "
             " [-G txgain]"
             " [-o offset]"
-            " [-O offsetfile]"
             " [-T filter_taps_file]"
             " [-a gain]"
             " [-c clockrate]"
@@ -66,9 +65,7 @@ void printUsage(char* progName)
     fprintf(out, "-F frequency:  Set the transmit frequency when using UHD output. (mandatory option when using UHD)\n");
     fprintf(out, "-G txgain:     Set the transmit gain for the UHD driver (default: 0)\n");
     fprintf(out, "-o:            (UHD only) Set the timestamp offset added to the timestamp in the ETI. The offset is a double.\n");
-    fprintf(out, "-O:            (UHD only) Set the file containing the timestamp offset added to the timestamp in the ETI.\n"
-                                 "The file is read every six seconds, and must contain a double value.\n");
-    fprintf(out, "                  Specifying either -o or -O has two implications: It enables synchronous transmission,\n"
+    fprintf(out, "                  Specifying this option has two implications: It enables synchronous transmission,\n"
                  "                  requiring an external REFCLK and PPS signal and frames that do not contain a valid timestamp\n"
                  "                  get muted.\n\n");
     fprintf(out, "-T taps_file:  Enable filtering before the output, using the specified file containing the filter taps.\n");