From 7ac0d2877fd4bccc3b28653e086849b49b655ace Mon Sep 17 00:00:00 2001 From: "Matthias P. Braendli" Date: Fri, 12 Jun 2015 16:14:17 +0200 Subject: Add some sort of initial gps check --- src/OutputUHD.cpp | 221 ++++++++++++++++++++++++++++++++++++------------------ src/OutputUHD.h | 17 ++++- 2 files changed, 162 insertions(+), 76 deletions(-) diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp index cc6c0b6..fdf9cda 100644 --- a/src/OutputUHD.cpp +++ b/src/OutputUHD.cpp @@ -31,6 +31,7 @@ #include "PcDebug.h" #include "Log.h" #include "RemoteControl.h" +#include "Utils.h" #include @@ -247,7 +248,7 @@ OutputUHD::OutputUHD( uwd.max_gps_holdover = myConf.maxGPSHoldoverTime; - SetDelayBuffer(config.dabMode); + SetDelayBuffer(myConf.dabMode); shared_ptr b(new barrier(2)); mySyncBarrier = b; @@ -267,28 +268,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. @@ -455,8 +454,10 @@ void UHDWorker::process() last_pps = 2.0; // Variables needed for GPS fix check - last_gps_fix_check = 0.0; - num_checks_without_gps_fix = 0; + 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 == 0 uhd::stream_args_t stream_args("fc32"); //complex floats @@ -471,6 +472,8 @@ void UHDWorker::process() num_underflows = 0; num_late_packets = 0; + bool wait_for_gps = uwd->check_gpsfix; + while (uwd->running) { fct_discontinuity = false; md.has_time_spec = false; @@ -494,7 +497,19 @@ void UHDWorker::process() "UHDWorker.process: workerbuffer is neither 0 nor 1 !"); } - handle_frame(frame); + // Don't start transmitting before we confirm the GPS is locked. + if (wait_for_gps) { + initial_gps_check(); + + if (num_checks_without_gps_fix == 0) { + wait_for_gps = false; + } + } + else { + handle_frame(frame); + + check_gps(); + } // swap buffers workerbuffer = (workerbuffer + 1) % 2; @@ -503,8 +518,6 @@ void UHDWorker::process() void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame) { - const double gps_fix_check_interval = 10.0; // seconds - pps_offset = frame->ts.timestamp_pps_offset; // Tx second from MNSC @@ -554,57 +567,6 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame) double 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(); - - 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; - } - - 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()); - } - } - } - else { - // Checking the sensor here takes too much - // time, it has to be done in a separate thread. - gps_fix_pt = boost::packaged_task( - 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)); - } - } - if (uwd->sourceContainsTimestamp) { if (!frame->ts.timestamp_valid) { @@ -728,6 +690,117 @@ void UHDWorker::tx_frame(const struct UHDWorkerFrameData *frame) } } +void UHDWorker::initial_gps_check() +{ + if (first_gps_fix_check.tv_sec == 0) { + etiLog.level(info) << "Waiting for GPS fix"; + + if (clock_gettime(CLOCK_MONOTONIC, &first_gps_fix_check) != 0) { + stringstream ss; + ss << "clock_gettime failure: " << strerror(errno); + throw std::runtime_error(ss.str()); + } + } + + 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()); + } + } + + struct timespec now; + if (clock_gettime(CLOCK_MONOTONIC, &now) != 0) { + stringstream ss; + ss << "clock_gettime failure: " << strerror(errno); + throw std::runtime_error(ss.str()); + } + + long delta_us = timespecdiff_us(time_last_frame, now); + long wait_time_us = uwd->fct_increment * 24000L; // TODO ugly + + 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::check_gps() +{ + 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()); + } + + // 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; + + // 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(); + + 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; + } + + 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()); + } + } + } + else { + // Checking the sensor here takes too much + // time, it has to be done in a separate thread. + gps_fix_pt = boost::packaged_task( + 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)); + } + } +} + void UHDWorker::print_async_metadata(const struct UHDWorkerFrameData *frame) { #if FAKE_UHD == 0 diff --git a/src/OutputUHD.h b/src/OutputUHD.h index 49489e3..bc01223 100644 --- a/src/OutputUHD.h +++ b/src/OutputUHD.h @@ -172,16 +172,24 @@ class UHDWorker { double last_pps; // GPS Fix check variables - double last_gps_fix_check; 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 gps_fix_pt; boost::unique_future gps_fix_future; boost::thread gps_fix_task; - // Transmit timeout static const double tx_timeout = 20.0; + // 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 process(); void process_errhandler(); @@ -189,6 +197,11 @@ class UHDWorker { void handle_frame(const struct UHDWorkerFrameData *frame); void tx_frame(const struct UHDWorkerFrameData *frame); + void check_gps(); + + void set_usrp_time_gps(); + + void initial_gps_check(); struct UHDWorkerData *uwd; boost::thread uhd_thread; -- cgit v1.2.3