uhd_io.cpp

#include <stdio.h>
#include <string>
#include <signal.h>
#include <atomic>
#include <vector>
#include <mutex>
#include "uhd_thread.h"
#include "cmdlineparser.h"
#include "tb_interface.h"
#include <uhd.h>
using namespace TASKBUS;
using namespace std;
typedef short SPTYPE;
static std::atomic<bool> stop_signal_called (false);
#define UHD_DO(X) \
{\
	uhd_error e = (X);\
	char buf_errs[512];\
	if (e) { snprintf(buf_errs,sizeof(buf_errs),"Error in line %d, NO %d.",__LINE__,e);\
	std::string ev = __FILE__;\
	ev += ":";\
	ev += buf_errs;\
	return_code = 1;\
	throw ev;\
	}\
	};
void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}

/*!
 * \brief The tag_timestmp struct
 * Defines the fine-timestamp used for both RX and TX.
 * TM Stamp is in unit Seconds.
 */
struct tag_timestmp{
	quint64 sec;
	double frag;
};
/*!
 * \brief The tag_sig_block struct
 * RX Buf Node struct,with timestamp and block buffer.
 * RX Buf is formed with RX Buf Nodes,
 * each node contains several sample points for each channel.
 */
struct tag_sig_block{
	struct tag_timestmp{
		quint64 sec;
		double frag;
	} tmstmp;
	//valid sample points in buffer. Buffer is pre-alloced.
	size_t ch_len;
	//buffer for each channel
	std::vector<std::vector<unsigned char> > ch_data;
	//buffer prt for each channel.
	std::vector<void *> ch_ptr;
};

/*!
 * \brief The tag_tx_plain struct
 * Time plan for TX.
 */
struct tag_tx_plain{
	struct tag_timestmp{
		quint64 sec;
		double frag;
	} tmstmp;
	qint64 length_left;
};

void cmd_dealing(uhd_usrp_handle usrp,std::vector<unsigned char> & cmd);


static char error_string[512];
int do_iio(const cmdlineParser & args)
{
	int return_code = EXIT_SUCCESS;
	std::string dev_args = args.toString("dev_args","");

	const long long time_sync =2;//For multi channel-Sync;
	//RX Channels, Default 0
	std::vector<size_t> rx_channels = args.toUInt64Array("rx_channels");
	adjuestUnit<size_t>(rx_channels,0);
	int rx_channel_count = rx_channels.size();
	if (rx_channel_count>2)
		rx_channel_count = 2;
	//RX Sample Rate in Hz
	double rx_rate		=   args.toDouble("rx_rate",1)*1e6;
	//RX Freq in MHz --> Hz
	std::vector<double> rx_freq      =   args.toDoubleArray("rx_rf");
	adjuestUnit<double> (rx_channel_count,rx_freq,1000,1e6);
	//Rx Gain
	std::vector<double> rx_gain      =   args.toDoubleArray("rx_gain");
	adjuestUnit<double> (rx_channel_count,rx_gain,30);
	//AGC
	std::vector<int>    rx_agc		=   args.toIntArray("rx_agc");
	adjuestUnit<int> (rx_channel_count,rx_agc,0);
	//RX Bw in MHz --> Hz
	std::vector<double> rx_bw		=   args.toDoubleArray("rx_bw");
	adjuestUnit<double> (rx_channel_count,rx_bw ,0 ,1e6);
	//RX Antenna
	std::vector<std::string> rx_atn  =   args.toStringArray("rx_atn");

	adjustString (rx_channel_count,rx_atn,"RX2");
	//RX switch
	bool  rx_on         =   args.toInt("rx_on",0)?true:false;
	//TX Channels, Default 0
	std::vector<size_t> tx_channels = args.toUInt64Array("tx_channels");
	adjuestUnit<size_t>(tx_channels,0);
	int tx_channel_count = tx_channels.size();
	if(tx_channel_count>2)
		tx_channel_count = 2;
	//TX Sample Rate in MHz -->Hz
	double tx_rate		=   args.toDouble("tx_rate",1.0)*1e6;
	//TX Freq in MHz -->Hz
	std::vector<double> tx_freq		=   args.toDoubleArray("tx_rf");
	adjuestUnit<double> (tx_channel_count,tx_freq,2000,1e6);
	//Tx Gain in dB
	std::vector<double> tx_gain      =   args.toDoubleArray("tx_gain");
	adjuestUnit<double> (tx_channel_count,tx_gain,30);
	//TX BW in MHz -->Hz
	std::vector<double> tx_bw		=   args.toDoubleArray("tx_bw");
	adjuestUnit<double> (tx_channel_count,tx_bw,0 ,1e6);
	//TX Antenna
	std::vector<std::string> tx_atn  =   args.toStringArray("tx_atn");
	adjustString (rx_channel_count,tx_atn,"TX/RX");
	//TX switch
	bool  tx_on         =   args.toInt("tx_on",0)?true:false;
	if (!rx_on && !tx_on)
	{
		fprintf(stderr,"Both RX and TX switch are OFF, nothing can do.\n");
		return 0;
	}
	const quint32 instance	  = args.toUInt("instance",0);
	const quint32 i_wav_rx[2]	  = {args.toUInt("wav_rx0",0),args.toUInt("wav_rx1",0)};
	const quint32 i_wav_tx[2]	  = {args.toUInt("wav_tx0",0),args.toUInt("wav_tx1",0)};
	const quint32 i_rx_tm = args.toInt("rx_time",0);
	const quint32 i_tx_tm = args.toInt("tx_time",0);
	const quint32 i_txWaterMark = args.toInt("tx_mark",0);

	//设备句柄
	uhd_usrp_handle usrp = nullptr;
	uhd_rx_streamer_handle rx_streamer = nullptr;
	uhd_rx_metadata_handle rx_meta = nullptr;
	uhd_tx_streamer_handle tx_streamer = nullptr;
	uhd_tx_metadata_handle tx_meta = nullptr;


	//发射缓存要提前配置，以便及时反馈水位
	//Time plan for tx
	std::list<tag_tx_plain> tx_plan;
	std::mutex mtx_plan;

	//ring buffer In IQ Points, TX io
	const quint64 sz_buffer_tx = 256 * 1024 * 1024;
	std::vector<std::vector<SPTYPE> > buf_tx_array;
	std::vector<quint64> tx_pos;
	std::vector<quint64> stdin_pos;
	std::vector<void *> tx_buff_ptr;
	size_t num_sps_sent = 0;
	for (int ch = 0;ch<tx_channel_count;++ch)
	{
		buf_tx_array.push_back(std::vector<SPTYPE>(sz_buffer_tx + 1024*1024*32,0));
		stdin_pos.push_back(0);
		tx_pos.push_back(0);
		tx_buff_ptr.push_back(0);
	}
	std::mutex push_mtx;
	std::mutex * pMtx = (i_wav_rx[0]+i_wav_rx[1]+i_txWaterMark)>0?&push_mtx:nullptr;

	qint64 total_points_left = -1;
	//Define StdIn Thread, Producer
	std::function<void()> thread_stdin = [&]()->void
	{
		try{
			//Read STDIN
			while (!stop_signal_called)
			{
				subject_package_header header;
				std::vector<unsigned char> packagedta = pull_subject(&header);
				if (!is_valid_header(header))
				{
					fprintf(stderr,"Recived BAD Command.");
					fflush(stderr);
					QThread::msleep(100);
					continue;
				}
				if (packagedta.size())
				{
					if ( is_control_subject(header))
					{
						//收到命令进程退出的广播消息,退出
						if (strstr((const char *)packagedta.data(),"function=quit;")!=nullptr)
						{
							fprintf(stderr,"Recived Quit Command.");
							fflush(stderr);
							stop_signal_called = true;
						}
						else
							cmd_dealing(usrp,packagedta);
					}
					else
					{
						if (header.subject_id==i_tx_tm)
						{
							if (packagedta.size()==sizeof(tag_tx_plain))
							{
								tag_tx_plain * plan = (tag_tx_plain *) (packagedta.data());
								if (total_points_left<=0)
									total_points_left = plan->length_left;
								fprintf(stderr,"plan %d points\n",plan->length_left);
								mtx_plan.lock();
								tx_plan.push_back(*plan);
								mtx_plan.unlock();
							}
							else
								fprintf(stderr,"Error Timeplan subject size %d recieved. Expected should be %d.\n",
										(int)packagedta.size(),(int)sizeof(tag_tx_plain));

						}
						else
						{
							fprintf(stderr,"recv %d points\n",packagedta.size()/2/sizeof(SPTYPE));
							for (int ch = 0; ch < tx_channel_count;++ch)
							{
								if (header.subject_id == i_wav_tx[ch])
								{
									SPTYPE * buf_tx_ptr = buf_tx_array[ch].data();
									quint64 sps_rev = packagedta.size()/2/sizeof(SPTYPE);
									short * iq = (short *)(packagedta.data());
									memcpy(&buf_tx_ptr[(stdin_pos[ch] * 2)% sz_buffer_tx],
											iq,  sps_rev * sizeof(SPTYPE)*2
											);
									const quint64 bgnext = ((stdin_pos[ch] + sps_rev) * 2) % sz_buffer_tx;
									if (bgnext < sps_rev * 2 && bgnext>0)
									{
										memcpy(buf_tx_ptr, buf_tx_ptr + sz_buffer_tx, bgnext * sizeof(SPTYPE) );
									}
									stdin_pos[ch] += sps_rev;
								}//end if (header.subject_id == i_wav_tx[ch])
							}//end for (int ch = 0; ch < tx_channel_count;++ch)
							if (total_points_left<0&&i_txWaterMark>0)
							{
								qint64 watM = stdin_pos[0] - tx_pos[0];
								if (tx_channel_count>1)
								{
									qint64 wat2 = stdin_pos[1] - tx_pos[1];
									if (wat2 < watM)
										watM = wat2;
								}
								TASKBUS::push_subject(
											i_txWaterMark,
											instance,
											sizeof(qint64),
											(unsigned char *) &watM,
											pMtx);
							}//end if (total_points_left<0)
						}//end else if (header.subject_id==i_tx_tm)
					}//end else if if ( is_control_subject(header))
				}//end if (packagedta.size())
			}//end while (!stop_signal_called)
		}//end try
		catch (std::string er)
		{
			fputs(er.c_str(),stderr);
			stop_signal_called = true;
		}
	};//end std::function<void()> thread_stdin = [&]()->void
	uhd_io_thread th_wav_stdin(thread_stdin,0);
	th_wav_stdin.start(QThread::HighestPriority);

	try{
		fprintf(stderr, "Creating USRP with args \"%s\"...\n", dev_args.c_str());
		UHD_DO(uhd_usrp_make(&usrp, dev_args.c_str()));

		fflush(stderr);
		// Ctrl+C will exit loop
		signal(SIGINT, &sigint_handler);
		if (rx_on)
		{
			// Create RX streamer
			UHD_DO(uhd_rx_streamer_make(&rx_streamer));
			// Create RX metadata
			UHD_DO(uhd_rx_metadata_make(&rx_meta));
			//RXPer-Channel Settings
			for (int ch = 0; ch < rx_channel_count;++ch)
			{
				fprintf(stderr, "Setting RX Channel: %d=%lu...\n",ch, rx_channels[ch]);
				UHD_DO(uhd_usrp_set_rx_antenna(usrp,rx_atn[ch].c_str(),ch));
				fprintf(stderr, "Setting RX Rate: %lf...\n", rx_rate);
				UHD_DO(uhd_usrp_set_rx_rate(usrp, rx_rate, rx_channels[ch]));
				// See what rate actually is
				UHD_DO(uhd_usrp_get_rx_rate(usrp, rx_channels[ch], &rx_rate));
				fprintf(stderr, "Actual RX Rate: %lf...\n", rx_rate);
				// Set gain
				fprintf(stderr, "Setting RX Gain: %lf dB...\n", rx_gain[ch]);
				UHD_DO(uhd_usrp_set_rx_gain(usrp, rx_gain[ch], rx_channels[ch], ""));
				// See what gain actually is
				UHD_DO(uhd_usrp_get_rx_gain(usrp, rx_channels[ch], "", &rx_gain[ch]));
				fprintf(stderr, "Actual RX Gain: %lf...\n", rx_gain[ch]);
				if (rx_agc[ch])
				{
					uhd_usrp_set_rx_agc(usrp,true,rx_channels[ch]);
					uhd_usrp_set_rx_dc_offset_enabled(usrp,true,rx_channels[ch]);
				}
				else
					annouce_handle(
								args.toString("instance","0"),
								"0",
								QString("rxgain_%1").arg(ch).toStdString(),
								QString("%1").arg(rx_gain[ch]).toStdString(),
								QString("rxgain_%1").arg(ch).toStdString(),
								"int",
								"0:100"
								   );
				// Set frequency
				// Create other necessary structs
				uhd_tune_request_t rx_tune_request =
				{
					/*.target_freq = */rx_freq[ch],
					/*.rf_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
					/*.rf_freq = */0,
					/*.dsp_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
					/*.dsp_freq = */0,
					/*.args = */0
				};
				uhd_tune_result_t rx_tune_result;

				fprintf(stderr, "Setting RX frequency: %lf MHz...\n", rx_freq[ch]/1e6);
				UHD_DO(uhd_usrp_set_rx_freq(usrp, &rx_tune_request, rx_channels[ch], &rx_tune_result));
				// See what frequency actually isrx_streamer
				UHD_DO(uhd_usrp_get_rx_freq(usrp, rx_channels[ch], &rx_freq[ch]));
				fprintf(stderr, "Actual RX frequency: %lf MHz...\n", rx_freq[ch] / 1e6);
				annouce_handle(
							args.toString("instance","0"),
							"0",
							QString("rxfreq_%1").arg(ch).toStdString(),
							QString("%1").arg(rx_freq[ch]).toStdString(),
							QString("rxfreq_%1").arg(ch).toStdString(),
							"double",
							"0:6000000000","100000","0"
							   );
				fprintf(stderr, "Setting RX Bandwidth: %lf MHz...\n", rx_bw[ch]/1e6);
				UHD_DO(uhd_usrp_set_rx_bandwidth(usrp, rx_bw[ch], rx_channels[ch]));
				//Band
				UHD_DO(uhd_usrp_get_rx_bandwidth(usrp, rx_channels[ch], &rx_bw[ch]));
				fprintf(stderr, "Actual RX Bandwidth: %lf MHz...\n", rx_bw[ch] / 1e6);
				fflush(stderr);
			}//end for (int ch = 0; ch < rx_channel_count;++ch)
			annouce_handle(
						args.toString("instance","0"),
						"0",
						"rx_time",
						"",
						"rx_time",
						"string"
						   );
		}//end if (rx_on)
		if (tx_on)
		{
			// Create TX streamer
			UHD_DO(uhd_tx_streamer_make(&tx_streamer));
			// Create TX metadata
			UHD_DO(uhd_tx_metadata_make(&tx_meta, false, 0,0, false, false));

			//TX Per-Channel Settings
			for (int ch = 0;ch < tx_channel_count;++ch)
			{
				fprintf(stderr, "Setting TX Channel: %d=%lu...\n",ch, tx_channels[ch]);
				UHD_DO(uhd_usrp_set_tx_antenna(usrp,tx_atn[ch].c_str(),ch));
				// Set rate
				fprintf(stderr, "Setting TX Rate: %lf...\n", tx_rate);
				UHD_DO(uhd_usrp_set_tx_rate(usrp, tx_rate, tx_channels[ch]));
				// See what rate actually is
				UHD_DO(uhd_usrp_get_tx_rate(usrp, tx_channels[ch], &tx_rate));
				fprintf(stderr, "Actual TX Rate: %lf...\n\n", tx_rate);
				// Set gain
				fprintf(stderr, "Setting TX Gain: %lf db...\n", tx_gain[ch]);
				UHD_DO(uhd_usrp_set_tx_gain(usrp, tx_gain[ch],tx_channels[ch], ""));
				// See what gain actually is
				UHD_DO(uhd_usrp_get_tx_gain(usrp, tx_channels[ch], "", &tx_gain[ch]));
				fprintf(stderr, "Actual TX Gain: %lf...\n", tx_gain[ch]);
				annouce_handle(
							args.toString("instance","0"),
							"0",
							QString("txgain_%1").arg(ch).toStdString(),
							QString("%1").arg(tx_gain[ch]).toStdString(),
							QString("txgain_%1").arg(ch).toStdString(),
							"int",
							"0:100"
							   );

				// Create other necessary structs for TX
				uhd_tune_request_t tx_tune_request = {
					/*.target_freq = */tx_freq[ch],
					/*.rf_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
					/*.rf_freq = */0,
					/*.dsp_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
					/*.dsp_freq = */0,
					/*.args = */0
				};
				uhd_tune_result_t tx_tune_result;
				// Set frequency
				fprintf(stderr, "Setting TX frequency: %f MHz...\n", tx_freq[ch] / 1e6);
				UHD_DO(uhd_usrp_set_tx_freq(usrp, &tx_tune_request, tx_channels[ch], &tx_tune_result));

				// See what frequency actually is
				UHD_DO(uhd_usrp_get_tx_freq(usrp, tx_channels[ch], &tx_freq[ch]));
				fprintf(stderr, "Actual TX frequency: %f MHz...\n", tx_freq[ch] / 1e6);
				annouce_handle(
							args.toString("instance","0"),
							"0",
							QString("txfreq_%1").arg(ch).toStdString(),
							QString("%1").arg(tx_freq[ch]).toStdString(),
							QString("txfreq_%1").arg(ch).toStdString(),
							"double",
							"0:6000000000","100000","0"
							   );
				//Band
				fprintf(stderr, "Setting TX Bandwidth: %f MHz...\n", tx_bw[ch]/1e6);
				UHD_DO(uhd_usrp_set_tx_bandwidth(usrp, tx_bw[ch],tx_channels[ch]));

				UHD_DO(uhd_usrp_get_tx_bandwidth(usrp, tx_channels[ch], &tx_bw[ch]));
				fprintf(stderr, "Actual TX Bandwidth: %f MHz...\n", tx_bw[ch] / 1e6);
			}//end for (int ch = 0;ch < tx_channel_count;++ch)
		}//end if (tx_on)
		char rx_cpu_format[] = "sc16";
		char rx_otw_format[] = "sc16";
		char rx_args[] = "";

		uhd_stream_args_t rx_stream_args = {
			/* .cpu_format = */rx_cpu_format,
			/* .otw_format = */rx_otw_format,
			/* .args = */rx_args,
			/* .channel_list = */rx_channels.data(),
			/*.n_channels = */rx_channel_count
		};
		uhd_stream_cmd_t rx_stream_cmd = {
			/*.stream_mode = */UHD_STREAM_MODE_START_CONTINUOUS,
			/*.num_samps = */0,
			/*.stream_now = */false,
			/*.time_spec_full_secs = */time_sync,
			/*.time_spec_frac_secs = */0
		};

		//char tx_cpu_format[] = "fc32";
		char tx_cpu_format[] = "sc16";
		char tx_otw_format[] = "sc16";
		char tx_args[] = "";

		uhd_stream_args_t tx_stream_args = {
			/*.cpu_format = */tx_cpu_format,
			/*.otw_format = */tx_otw_format,
			/*.args = */tx_args,
			/*.channel_list = */tx_channels.data(),
			/* .n_channels = */tx_channel_count
		};
		size_t rx_sps_buff = 0;
		size_t tx_sps_buff = 0;
		if (rx_on)
		{
			// Set up streamer
			UHD_DO(uhd_usrp_get_rx_stream(usrp, &rx_stream_args, rx_streamer));
			// Set up buffer
			UHD_DO(uhd_rx_streamer_max_num_samps(rx_streamer, &rx_sps_buff));
			fprintf(stderr, "RX Buffer size in samples: %zu\n", rx_sps_buff);
		}
		if (tx_on)
		{
			UHD_DO(uhd_usrp_get_tx_stream(usrp, &tx_stream_args, tx_streamer));
			// Set up buffer
			UHD_DO(uhd_tx_streamer_max_num_samps(tx_streamer, &tx_sps_buff));
			fprintf(stderr, "TX Buffer size in samples: %zu\n", tx_sps_buff);
			fflush(stderr);
		}

		//ring buffer in short inters
		const quint64 sz_buffer_rx = 16384;
		std::vector<tag_sig_block> buf_rx_list;
		//In Blocks, RX io
		quint64 rx_pos = 0,stdout_pos = 0;
		for (size_t buf_ct = 0; buf_ct < sz_buffer_rx; ++buf_ct)
		{
			tag_sig_block block;
			block.tmstmp.frag = 0;
			block.tmstmp.sec = 0;
			block.ch_len = 0;
			//IQ样点
			for (int ch = 0;ch<rx_channel_count;++ch)
				block.ch_data.push_back(std::vector<unsigned char>(rx_sps_buff*2*sizeof(SPTYPE),0));
			buf_rx_list.push_back(std::move(block));
		}
		for (size_t buf_ct = 0; buf_ct < sz_buffer_rx; ++buf_ct)
		{
			for (int ch = 0;ch<rx_channel_count;++ch)
				buf_rx_list[buf_ct].ch_ptr.push_back((void *)buf_rx_list[buf_ct].ch_data[ch].data());
		}

		//reset time
		uhd_usrp_set_time_now(usrp,0,0,0);
		//Define RX Thread, Producer
		std::function<void()> thread_rx = [&]()->void
		{
			try{
				// Issue stream command
				fprintf(stderr, "Issuing stream command.\n");
				UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
				//Read, RX
				while (!stop_signal_called) {
					buf_rx_list[rx_pos % sz_buffer_rx].tmstmp.sec = 0;
					buf_rx_list[rx_pos % sz_buffer_rx].tmstmp.frag = 0;
					// Handle data
					UHD_DO(uhd_rx_streamer_recv(
							   rx_streamer,
							   buf_rx_list[rx_pos % sz_buffer_rx].ch_ptr.data(),
							   rx_sps_buff,
							   &rx_meta, 5, true, &buf_rx_list[rx_pos % sz_buffer_rx].ch_len));
					uhd_rx_metadata_error_code_t error_code;
					UHD_DO(uhd_rx_metadata_error_code(rx_meta, &error_code));
					if(error_code != UHD_RX_METADATA_ERROR_CODE_NONE){
						fprintf(stderr, "Warning: Error code 0x%x 16was returned during streaming.\n", error_code);
						fputs(error_string,stderr);
					}
					else
					{
						bool hasTm;
						UHD_DO(uhd_rx_metadata_has_time_spec(rx_meta, &hasTm));
						if (hasTm)
						{
							int64_t rx_time_sec;
							double rx_time_frag;
							uhd_rx_metadata_time_spec(rx_meta,
													  &rx_time_sec, & rx_time_frag);
							buf_rx_list[rx_pos % sz_buffer_rx].tmstmp.sec = rx_time_sec;
							buf_rx_list[rx_pos % sz_buffer_rx].tmstmp.frag = rx_time_frag;
						}
					}
					++rx_pos;
				}
			}
			catch (std::string er)
			{
				fputs(er.c_str(),stderr);
				stop_signal_called = true;
			}
		};//end std::function<void()> thread_rx = [&]()->void


		//Define StdOut Thread, Consumer
		std::function<void()> thread_stdout = [&]()->void
		{
			try{
				unsigned int ct_br = 0;
				//Read, RX
				while (!stop_signal_called)
				{
					if (stdout_pos >= rx_pos)
					{
						QThread::msleep(1);
						continue;
					}
					if (++ct_br % 1000==0)
						set_handle(args.toString("instance","0"),
									"0",
									"rx_time",
									QString("%1.%2")
								   .arg(buf_rx_list[stdout_pos % sz_buffer_rx].tmstmp.sec)
									.arg(buf_rx_list[stdout_pos % sz_buffer_rx].tmstmp.frag).toStdString());
					if (i_rx_tm)
					{
						TASKBUS::push_subject(
									i_rx_tm,
									instance,
									sizeof(tag_timestmp),
									(unsigned char *) &buf_rx_list[stdout_pos % sz_buffer_rx].tmstmp,
									pMtx);
					}
					for (int ch = 0;ch<rx_channel_count;++ch)
					{
						if (i_wav_rx[ch]>0)
						{
							tag_sig_block & dta = buf_rx_list[stdout_pos % sz_buffer_rx];
							TASKBUS::push_subject(
										i_wav_rx[ch],
										instance,
										buf_rx_list[stdout_pos % sz_buffer_rx].ch_len * 2 * sizeof(SPTYPE),
										(unsigned char *) dta.ch_data[ch].data(),
										pMtx);
						}
					}
					++stdout_pos;
				}
			}
			catch (std::string er)
			{
				fputs(er.c_str(),stderr);
				stop_signal_called = true;
			}
		};//end std::function<void()> thread_stdout = [&]()->void

		const int tx_frame = 10000;
		//Define thread_tx, Consumer
		std::function<void()> thread_tx = [&]()->void
		{
			try{
				while (!stop_signal_called)
				{
					bool can_send = true;
					if (total_points_left==0)
					{
						QThread::msleep(1);
						continue;
					}
					else if (total_points_left<0)
					{
						if (i_txWaterMark>0)
						{
							qint64 watM = stdin_pos[0] - tx_pos[0];
							if (tx_channel_count>1)
							{
								qint64 wat2 = stdin_pos[1] - tx_pos[1];
								if (wat2 < watM)
									watM = wat2;
							}
							TASKBUS::push_subject(
										i_txWaterMark,
										instance,
										sizeof(qint64),
										(unsigned char *) &watM,
										pMtx);

						}
					}
					for (int ch=0;ch<tx_channel_count && can_send;++ch)
					{
						SPTYPE * buf_tx_ptr = buf_tx_array[ch].data();
						//连续模式数据不够
						if (stdin_pos[ch] < (tx_sps_buff + tx_frame)*10 && total_points_left<0)
						{
							can_send = false;
							continue;
						}
						//连续模式数据不够
						if (tx_pos[ch] + (long long) (tx_sps_buff + tx_frame) >= stdin_pos[ch] && total_points_left<0)
						{
							can_send = false;
							continue;
						}
						//突发模式数据不够
						if (total_points_left >0 &&  tx_pos[ch] + (long long) (tx_sps_buff ) > stdin_pos[ch]
								&&  tx_pos[ch] + (long long) (total_points_left ) > stdin_pos[ch]
								)
						{
							fprintf(stderr,"total_points LESS! for %d points\n",total_points_left);
							can_send = false;
							continue;
						}
						const quint64 bgnext = ((tx_pos[ch] + tx_sps_buff) * 2) % sz_buffer_tx;
						//Dealing with the tail
						if (bgnext < tx_sps_buff * 2 && bgnext>0)
						{
							memcpy( buf_tx_ptr + sz_buffer_tx, buf_tx_ptr, bgnext * sizeof(SPTYPE) );
						}
						SPTYPE * tx_buff = &buf_tx_ptr[(tx_pos[ch] * 2) % sz_buffer_tx];
						tx_buff_ptr[ch] = (void *)tx_buff;
					}
					if (!can_send)
					{
						QThread::msleep(1);
					}
					//Continous
					else if (total_points_left<0)
					{
						UHD_DO(uhd_tx_streamer_send(tx_streamer,
													(const void **)tx_buff_ptr.data(),
													tx_sps_buff, &tx_meta,0.1, &num_sps_sent));
						//tx_meta->tx_metadata_ctmstmppp.start_of_burst = false;
						for(int ch = 0;ch<tx_channel_count;++ch)
							tx_pos[ch] += num_sps_sent;
					}
					//Burst
					else
					{
						assert(total_points_left>0);
						uhd_tx_metadata_handle tx_meta_burst = nullptr;
						size_t emit_points = tx_sps_buff;
						//Mutex is not needed.
						tag_tx_plain * plan = &tx_plan.front();
						//First pack
						if (plan->length_left==total_points_left)
						{
							//fprintf(stderr,"total_points start for %d points\n",total_points_left);
							if (total_points_left <= (qint64)tx_sps_buff)
							{
								UHD_DO(uhd_tx_metadata_make(&tx_meta_burst,
															plan->tmstmp.sec?true:false,
															plan->tmstmp.sec,plan->tmstmp.frag,true,true));
							}
							else
							{
								UHD_DO(uhd_tx_metadata_make(&tx_meta_burst,
															plan->tmstmp.sec?true:false,
															plan->tmstmp.sec,plan->tmstmp.frag,true,false));
							}
						}
						//Last pack
						else if (total_points_left <= (qint64)tx_sps_buff)
						{
							//fprintf(stderr,"total_points end for %d points\n",total_points_left);
							emit_points = total_points_left;
							UHD_DO(uhd_tx_metadata_make(&tx_meta_burst,false,0,0,false,true));
						}
						else
						{
							//fprintf(stderr,"total_points middle for %d points\n",total_points_left);
							UHD_DO(uhd_tx_metadata_make(&tx_meta_burst,false,0,0,false,false));
						}

						UHD_DO(uhd_tx_streamer_send(tx_streamer,
													(const void **)tx_buff_ptr.data(),
													emit_points, &tx_meta_burst,600, &num_sps_sent));
						//Middle of burst
						for(int ch = 0;ch<tx_channel_count;++ch)
							tx_pos[ch] += num_sps_sent;
						total_points_left -= num_sps_sent;
						assert(total_points_left>=0);
						if(total_points_left<=0)
						{
							//fprintf(stderr,"total_points over\n");
							mtx_plan.lock();
							assert(tx_plan.size());
							tx_plan.pop_front();
							if (tx_plan.size())
								total_points_left = tx_plan.front().length_left;
							mtx_plan.unlock();
						}
						UHD_DO(uhd_tx_metadata_free(&tx_meta_burst));
						//fprintf(stderr,"total_points left %d points\n",total_points_left);
					}//end else if (total_points_left>0)

				}//end while (!stop_signal_called)
			}
			catch(std::string er)
			{
				fputs(er.c_str(),stderr);
				stop_signal_called = true;
			}
		};//end std::function<void()> thread_tx = [&]()->void

		uhd_io_thread th_wav_rx(thread_rx,0);
		uhd_io_thread th_wav_stdout(thread_stdout,0);
		uhd_io_thread th_wav_tx(thread_tx,0);

		if (rx_on)
		{
			th_wav_rx.start(QThread::HighestPriority);
			th_wav_stdout.start(QThread::HighestPriority);
		}
		if (tx_on)
		{
			th_wav_tx.start(QThread::HighestPriority);
		}

		while (!stop_signal_called)
		{
			fprintf(stderr,"\nRX:");
			fprintf (stderr,"%lld,%lld;",rx_pos,stdout_pos);
			fprintf(stderr,"\n");
			fprintf(stderr,"TX:");
			for (int ch = 0;ch<tx_channel_count;++ch)
				fprintf (stderr,"%d:%lld,%lld;",ch,stdin_pos[ch],tx_pos[ch]);
			fprintf(stderr,"\n");
			fflush(stderr);
			QThread::msleep(1000);
		}

		th_wav_rx.wait();
		th_wav_stdout.wait();
		th_wav_tx.wait();
	}//end top try
	catch(std::string s)
	{
		fputs(s.c_str(),stderr);
	}

	fflush(stderr);

	if (tx_streamer) uhd_tx_streamer_free(&tx_streamer);
	if (rx_streamer) uhd_rx_streamer_free(&rx_streamer);
	if (tx_meta) uhd_tx_metadata_free(&tx_meta);
	if (rx_meta) uhd_rx_metadata_free(&rx_meta);

	if(return_code != EXIT_SUCCESS && usrp != nullptr){
		uhd_usrp_last_error(usrp, error_string, 512);
		fprintf(stderr, "USRP reported the following error: %s\n", error_string);
	}
	uhd_usrp_free(&usrp);

	th_wav_stdin.wait();

	fprintf(stderr, (return_code ? "Failure\n" : "Success\n"));
	fflush(stderr);
	return return_code;
}

void cmd_dealing(uhd_usrp_handle usrp,std::vector<unsigned char> & cmd)
{
	if (!usrp)
		return ;
	std::map<std::string,std::string> mcmd = TASKBUS::ctrlpackage_to_map(cmd);
	if (mcmd["function"]=="handle_set")
	{
		if (mcmd["handle"].find("rxgain_")!=mcmd["handle"].npos)
		{
			int channel = atoi(mcmd["handle"].substr(7).c_str());
			int gain = atoi(mcmd["value"].c_str());
			uhd_usrp_set_rx_gain(usrp,gain,channel,"");
			double dgain = 0;
			uhd_usrp_get_rx_gain(usrp, channel, "", &dgain);
			fprintf(stderr, "Actual RX Gain: %lf...\n", dgain);

		}
		else if (mcmd["handle"].find("txgain_")!=mcmd["handle"].npos)
		{
			int channel = atoi(mcmd["handle"].substr(7).c_str());
			int gain = atoi(mcmd["value"].c_str());
			uhd_usrp_set_tx_gain(usrp,gain,channel,"");
			double dgain = 0;
			uhd_usrp_get_tx_gain(usrp, channel, "", &dgain);
			fprintf(stderr, "Actual TX Gain: %lf...\n", dgain);

		}
		else if (mcmd["handle"].find("rxfreq_")!=mcmd["handle"].npos)
		{
			int channel = atoi(mcmd["handle"].substr(7).c_str());
			double freq = atof(mcmd["value"].c_str());
			uhd_tune_request_t rx_tune_request =
			{
				/*.target_freq = */freq,
				/*.rf_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
				/*.rf_freq = */0,
				/*.dsp_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
				/*.dsp_freq = */0,
				/*.args = */0
			};
			uhd_tune_result_t rx_tune_result;
			fprintf(stderr, "Setting RX frequency: %lf MHz...\n", freq/1e6);
			uhd_usrp_set_rx_freq(usrp, &rx_tune_request, channel, &rx_tune_result);
		}
		else if (mcmd["handle"].find("txfreq_")!=mcmd["handle"].npos)
		{
			int channel = atoi(mcmd["handle"].substr(7).c_str());
			double freq = atof(mcmd["value"].c_str());
			uhd_tune_request_t tx_tune_request =
			{
				/*.target_freq = */freq,
				/*.rf_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
				/*.rf_freq = */0,
				/*.dsp_freq_policy = */UHD_TUNE_REQUEST_POLICY_AUTO,
				/*.dsp_freq = */0,
				/*.args = */0
			};
			uhd_tune_result_t rx_tune_result;
			fprintf(stderr, "Setting RX frequency: %lf MHz...\n", freq/1e6);
			uhd_usrp_set_tx_freq(usrp, &tx_tune_request, channel, &rx_tune_result);
		}
	}
}