diff options
| -rw-r--r-- | src/OutputUHD.cpp | 448 | ||||
| -rw-r--r-- | src/OutputUHD.h | 59 |
2 files changed, 264 insertions, 243 deletions
diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp index fdf9cda..b815a4c 100644 --- a/src/OutputUHD.cpp +++ b/src/OutputUHD.cpp @@ -61,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)), @@ -69,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 @@ -168,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; @@ -246,8 +224,6 @@ OutputUHD::OutputUHD( uwd.check_gpsfix = false; } - uwd.max_gps_holdover = myConf.maxGPSHoldoverTime; - SetDelayBuffer(myConf.dabMode); shared_ptr<barrier> b(new barrier(2)); @@ -271,7 +247,7 @@ OutputUHD::~OutputUHD() int transmission_frame_duration_ms(unsigned int dabMode) { switch (dabMode) { - // could happen when called from constructor and we take the mode from ETI + // could happen when called from constructor and we take the mode from ETI case 0: return 0; case 1: return 96; @@ -305,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); @@ -353,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) { @@ -407,43 +408,192 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut) } return uwd.bufsize; +} + +void OutputUHD::set_usrp_time() +{ + 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()); + } } -void UHDWorker::process_errhandler() +void OutputUHD::initial_gps_check() { - try { - process(); + 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()); + } } - catch (fct_discontinuity_error& e) { - etiLog.level(warn) << e.what(); - uwd->failed_due_to_fct = true; + + 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()); } - uwd->running = false; - uwd->sync_barrier.get()->wait(); - etiLog.level(warn) << "UHD worker terminated"; + 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 = 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++; + } } -// Check function for GPS fixtype -bool check_gps_fix_ok(struct UHDWorkerData *uwd) +void OutputUHD::check_gps() { - try { - std::string fixtype( - uwd->myUsrp->get_mboard_sensor("gps_fixtype", 0).to_pp_string()); + 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()); + } - if (fixtype.find("3d fix") == std::string::npos) { - etiLog.level(warn) << "OutputUHD: " << fixtype; + // 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; - return false; + // 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 > + 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)); } + } +} - return true; +//============================ UHD Worker ======================== + +void UHDWorker::process_errhandler() +{ + try { + process(); } - catch (uhd::lookup_error &e) { - etiLog.level(warn) << "OutputUHD: no gps_fixtype sensor"; - return false; + 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() @@ -453,12 +603,6 @@ void UHDWorker::process() pps_offset = 0.0; last_pps = 2.0; - // 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 == 0 uhd::stream_args_t stream_args("fc32"); //complex floats myTxStream = uwd->myUsrp->get_tx_stream(stream_args); @@ -472,8 +616,6 @@ 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; @@ -497,19 +639,7 @@ void UHDWorker::process() "UHDWorker.process: workerbuffer is neither 0 nor 1 !"); } - // 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(); - } + handle_frame(frame); // swap buffers workerbuffer = (workerbuffer + 1) % 2; @@ -518,6 +648,9 @@ void UHDWorker::process() void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame) { + // Transmit timeout + static const double tx_timeout = 20.0; + pps_offset = frame->ts.timestamp_pps_offset; // Tx second from MNSC @@ -641,6 +774,7 @@ void UHDWorker::handle_frame(const struct UHDWorkerFrameData *frame) 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); @@ -690,117 +824,6 @@ 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<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)); - } - } -} - void UHDWorker::print_async_metadata(const struct UHDWorkerFrameData *frame) { #if FAKE_UHD == 0 @@ -847,6 +870,9 @@ void UHDWorker::print_async_metadata(const struct UHDWorkerFrameData *frame) #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 bc01223..633de04 100644 --- a/src/OutputUHD.h +++ b/src/OutputUHD.h @@ -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; @@ -171,42 +168,18 @@ class UHDWorker { double pps_offset; double last_pps; - // 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; - - // 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(); - void print_async_metadata(const struct UHDWorkerFrameData *frame); 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; uhd::tx_streamer::sptr myTxStream; + + void process(); + void process_errhandler(); }; /* This structure is used as initial configuration for OutputUHD */ @@ -278,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; @@ -294,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 |