diff options
| -rw-r--r-- | src/OutputUHD.cpp | 531 | ||||
| -rw-r--r-- | src/OutputUHD.h | 26 |
2 files changed, 269 insertions, 288 deletions
diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp index 72d4976..cc6c0b6 100644 --- a/src/OutputUHD.cpp +++ b/src/OutputUHD.cpp @@ -449,63 +449,40 @@ bool check_gps_fix_ok(struct UHDWorkerData *uwd) void UHDWorker::process() { - int workerbuffer = 0; - time_t tx_second = 0; - double pps_offset = 0; - double last_pps = 2.0; - double usrp_time; + int workerbuffer = 0; + tx_second = 0; + pps_offset = 0.0; + last_pps = 2.0; // 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; - - // Asynchronous message statistics - int num_underflows = 0; - int num_late_packets = 0; - - //const struct timespec hundred_nano = {0, 100}; - - size_t sizeIn; - struct UHDWorkerFrameData* frame; - - size_t num_acc_samps; //number of accumulated samples - //int write_fail_count; - - // Transmit timeout - const double timeout = 20.0; + last_gps_fix_check = 0.0; + num_checks_without_gps_fix = 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); } @@ -517,306 +494,284 @@ 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) +{ + const double gps_fix_check_interval = 10.0; // seconds - /* 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; + + fct_discontinuity = true; + throw fct_discontinuity_error(); + } + } - // 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."); - } + expected_next_fct = (frame->ts.fct + uwd->fct_increment) % 250; + + // 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."); - } } + 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."); + } + } - usrp_time = uwd->myUsrp->get_time_now().get_real_secs(); + 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; + 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()) { + // 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_future.wait(); - gps_fix_task.join(); + gps_fix_task.join(); - 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++; + if (not gps_fix_future.get()) { + if (num_checks_without_gps_fix == 0) { + etiLog.level(alert) << + "OutputUHD: GPS Fix lost"; } - else { - if (num_checks_without_gps_fix) { - etiLog.level(info) << - "OutputUHD: GPS Fix recovered"; - } - num_checks_without_gps_fix = 0; + 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()); - } + 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) ); + } + 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_future = gps_fix_pt.get_future(); - gps_fix_task = boost::thread(boost::move(gps_fix_pt)); - } + gps_fix_task = boost::thread(boost::move(gps_fix_pt)); } + } - 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 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 } diff --git a/src/OutputUHD.h b/src/OutputUHD.h index bf5b27d..49489e3 100644 --- a/src/OutputUHD.h +++ b/src/OutputUHD.h @@ -160,9 +160,35 @@ class UHDWorker { } private: + // 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; + + // GPS Fix check variables + double last_gps_fix_check; + int num_checks_without_gps_fix; + 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; + 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); struct UHDWorkerData *uwd; boost::thread uhd_thread; |