[RX]

- improved STO correction

[Buffer]
- renamed readAt to peekAt
- added readAt which consumes


git-svn-id: http://moon:8086/svn/matlab/trunk@86 801c6759-fa7c-4059-a304-17956f83a07c

ofdm/buffer.m

@@ -1,6 +1,6 @@
 function [buf] = buffer()
 	
-	buf = struct('mem', [], 'ri', 0, 'wi', 0, 'obj', @()obj_(), 'len', @()len_(), 'read', @(len)read_(len), 'readAt', @(len, offset)readAt_(len, offset), 'setReadIndex', @(offset)setReadIndex_(offset), 'write', @(x)write_(x));
+	buf = struct('mem', [], 'ri', 0, 'wi', 0, 'obj', @()obj_(), 'len', @()len_(), 'read', @(len)read_(len), 'readAt', @(len, offset)readAt_(len, offset), 'peekAt', @(len, offset)peekAt_(len, offset), 'setReadIndex', @(offset)setReadIndex_(offset), 'setReadIndexRelative', @(offset)setReadIndexRelative_(offset), 'write', @(x)write_(x));
 
 	function b = obj_()
 		b = buf;
@@ -13,7 +13,7 @@ function [buf] = buffer()
 		end
 	end
 
-	function [x] = readAt_(len, offset)
+	function [x] = peekAt_(len, offset)
 		x = [];
 		
 		if (buf.ri+offset) >= 0
@@ -23,8 +23,15 @@ function [buf] = buffer()
 		end
 	end
 
+	function [x] = readAt_(len, offset)
+		x = peekAt_(len, offset);
+		if ~isempty(x)
+			buf.ri = buf.ri + len;
+		end
+	end
+
 	function [x] = read_(len)
-		x = readAt_(len, 0);
+		x = peekAt_(len, 0);
 		if ~isempty(x)
 			buf.ri = buf.ri + len;
 		end
@@ -34,6 +41,10 @@ function [buf] = buffer()
 		buf.ri = offset;
 	end
 
+	function setReadIndexRelative_(offset)
+		buf.ri = buf.ri+offset;
+	end
+
 	function write_(x)
 		len = length(x);
 		buf.mem(buf.wi+1:buf.wi+len, :) = x;

ofdm/ofdm_rx.m

@@ -112,7 +112,9 @@ fileRemain = wav_size;
 
 buf_recv = buffer();
 buf_Z = buffer();
-
+sample_offset = 0;
+sample_offset_integer = 0;
+sample_offset_fractional = 0;
 % --------------------------------------------------------------
 % Main loop
 % --------------------------------------------------------------
@@ -237,12 +239,15 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 			df_track = cfoCoarseFractional.df;
 			fprintf('Creating Symbols\n');
 			fprintf('Fine fractional CFO acquisition\n');
+			delta_theta = 0;
+			leadLag_f_z = 0;
+			leadLag_i_z = 0;
 		end
 		kn = fs/(2*pi*(1+Ng/N))/N;
 		sym_pilot = exp(j*2*pi*phi_pilot'/1024);
 		bin_pilot = c2i(N, carrier_pilot);
 
-		x = buf_recv.read(N+Ng);
+		x = buf_recv.readAt(N+Ng, -sample_offset_integer);
 		if (~isempty(x))
 
 			if equalizer.sync
@@ -254,7 +259,8 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 			
 			x = x.*exp(j*(2*pi*df_track/fs*(0:N+Ng-1)' + phi_track));
 			phi_track = mod(phi_track + 2*pi*df_track/fs*(N+Ng), 2*pi);
-			X = fft(x(Ng+1:N+Ng));
+			Fc = exp(j*2*pi*(0:N-1)/(N)*sample_offset_fractional);
+			X = fft(x(Ng+1:N+Ng)) .* [Fc(N/2:-1:1) Fc(N:-1:N/2+1)].';
 			buf_Z.write(X);
 			% CFO Fine acquisition
 			if ~equalizer.sync
@@ -279,8 +285,8 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 
 			if (frameTiming.n < ofdm_drm_params.nspf)
 				frameTiming.Z = 0;
-				Z1 = buf_Z.readAt(N, N*skip_syms);
+				Z1 = buf_Z.peekAt(N, N*skip_syms);
-				Z2 = buf_Z.readAt(N, N*(ofdm_drm_params.nspf+skip_syms));
+				Z2 = buf_Z.peekAt(N, N*(ofdm_drm_params.nspf+skip_syms));
 				kc = abs(Z1(bins)' * Z2(bins));
 				if valid
 					if kc > frameTiming.kc_max
@@ -309,6 +315,9 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 				
 				fprintf('Channel estimation and symbol reception\n');
 				[W_syms, W_pilots] = calcWiener(drm_mode, drm_bw);
+				sample_offset_fractional_ = [];
+				sample_offset_integer_ = [];
+				sample_offset_ = [];
 			end
 
 			Z = transpose(buf_Z.read(N));
@@ -318,9 +327,9 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 				[carrier_gain, phi_gain, mag_gain] = getRefGain(ofdm_drm_params, ofdm_spec_occ, equalizer.s);
 				carrier_gain_bins = c2i(N, carrier_gain);
 				H_ = Z(carrier_gain_bins)./(mag_gain.*exp(j*2*pi*phi_gain/1024));
-
+				Hc_ = (H_) .* exp(j*2*pi*carrier_gain_bins/(N)*(sample_offset_integer));
-				H_raw(carrier_gain_bins) = (H_);
+				H_raw(carrier_gain_bins) = Hc_;%.*Fc;
-				H_gain_cell{equalizer.m+1} = (H_);
+				H_gain_cell{equalizer.m+1} = Hc_;%.*Fc;
 
 				str = sprintf('Symbol %d, s=%d, m=%d, n=%d', equalizer.symbol_count, equalizer.s, equalizer.m, equalizer.n);
 
@@ -333,7 +342,7 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 					H_pilots = (H_gain)*W_pilots{equalizer.n+1};
 
 					% Get the correctly delayed symbols
-					Z = buf_Z.readAt(N, -N*(symbols_to_delay+1));
+					Z = buf_Z.peekAt(N, -N*(symbols_to_delay+1));
 	
 					sym = Z(carrier_indexes).';
 
@@ -370,9 +379,54 @@ while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave
 					sfoH = angle(sum_sfoH);
 					sfo_ = [sfo_ 2*fs/(2*pi*N)*(sfoH - sfoL)];
 
+					% STO
+					H = [H_syms zeros(1, N-length(H_syms))].';
+					Tg = Ng;
+					Tu = N;
+					Tgh = floor(Ng/2);
+					Tgs = floor(Ng);
+					max_delta_theta = Tgh;
+					h_absq = abs( ifft( transpose(H)) ).^2;	%note thate h_absq is mirrored in time direction because we use fft instead of ifft
+					h_filter_sto = sin([1:Tgs]/(Tgs+1)*pi).^(0.9);
+					[dummy, delta_theta] = max(filter(h_filter_sto , 1, [h_absq,h_absq] ) );	%find a window with maximum energy inside
+					delta_theta =  ( rem( N + Tgh + 1 - delta_theta + N*1.5, N ) - N/2 )*Tu/N;
+%					delta_theta = sign(delta_theta);
+					
+%					delta_theta = max(-1, min(1, delta_theta));
+					[sample_offset_fractional, leadLag_f_z] = leadLag(delta_theta, 0.001, 0.000, leadLag_f_z);
+					[sample_offset_integer, leadLag_i_z] = leadLag(delta_theta, 0.01, 0.000, leadLag_i_z);
+					%delta_theta = max(min(delta_theta,max_delta_theta),-max_delta_theta)+0;
+					%sample_offset = sample_offset - 0.1*delta_theta;
+%					sample_offset_integer = floor(sample_offset);
+					
+					sw_i = 1;
+					sw_f = 1;
+					sample_offset_fractional = sw_f *(sample_offset_fractional - sw_i*floor(sample_offset_fractional));
+					sample_offset_fractional_ = [sample_offset_fractional_ sample_offset_fractional];
+					sample_offset_integer = sw_i*floor(sample_offset_integer);
+					sample_offset_integer_ = [sample_offset_integer_ sample_offset_integer];
+
 					% equalize symbol
 					sym_eq = sym ./ H_syms;
-					if strcmpi(plot_mode, 'h_pilots')
+					if strcmp(plot_mode, 'h_pilots')
+						h = ifft(H_syms);
+						subplot(3, 1, 1)
+						plot(0:length(h)-1, real(h), 'b-'); grid; 
+						subplot(3, 1, 2)
+						plot(0:length(h_absq)-1, real(h_absq), 'b-'); grid; 
+						hold on;
+						for b=c2i(N, carrier_pilot)
+							x = [-delta_theta -delta_theta];
+							y = [0 2];
+							plot(x,y, 'r-');
+						end
+						hold off;
+						subplot(3, 1, 3)
+						plot(0:length(h_filter_sto)-1, real(h_filter_sto), 'b-'); grid; 
+						title(str);
+						pause(0.01);
+					end
+					if strcmp(plot_mode, 'H_pilots')
 						subplot(3, 1, 1)
 						plot(1:length(Hr), abs(Hr), 'b-', 1:length(H_pilots), abs(H_pilots), 'r-'); grid; 
 						subplot(3, 1, 2)
@@ -444,7 +498,12 @@ plot(0:length(df_track_)-1, df_track_); grid; title('df_{track}')
 hold on;
 
 figure;
-plot(0:length(sfo_)-1, sfo_); grid; title('SFO');
+subplot(3, 1, 1)
+plot(0:length(sfo_)-1, sfo_, '-'); grid; title('SFO');
+subplot(3, 1, 2)
+plot(0:length(sample_offset_fractional_)-1, sample_offset_fractional_, '-'); grid; title('STO_F');
+subplot(3, 1, 3)
+plot(0:length(sample_offset_integer_)-1, sample_offset_integer_, '-'); grid; title('STO_I');
 
 % --------------------------------------------------------------
 % Write wave files
@@ -479,3 +538,16 @@ fprintf(' done\n');
 % --------------------------------------------------------------
 function Y = spec_order(N, X)
     Y = [X(N/2+1:N)' X(1:N/2)']';
+
+function [y, zf] = leadLag(x, kI, kP, zi)
+	if isempty(zi) | isnumeric(zi)
+		zi = struct('state', 0);
+	end
+	
+	zi.state = kI*zi.state + x;
+	y = zi.state + x*kP;
+	
+	zf = zi;
+
+		
+