diff --git a/dsp.c b/dsp.c
index d3e6cd3..10ed283 100644
--- a/dsp.c
+++ b/dsp.c
@@ -63,6 +63,47 @@ void dsp_setmode(int mode)
 }
 
 
+/*
+ * S-Meter is based on RSSI, which is in fact the signal level in the preprocessor.
+ * The length of the (I,Q) vector is taken as reference for RSSI, where
+ * S value is highest bit set, i.e. RSSI of 512 corresponds with S-9 (S=log2(RSSI))
+ * This value was calibrated roughly by using the sam antenna and 
+ *   comparing an IC R71-E with my uSDR HW implementation and ADC_INT=8.
+ * +20dB means 10x the S-9 RSSI level, or >5120
+ * +40dB means 100x the S-9 RSSI level, or >51200
+ */
+#define S940	51200
+#define S930	16180
+#define S920	5120
+#define S910	1618
+#define S9		512
+#define S8		256
+#define S7		128
+#define S6		64
+#define S5		32
+#define S4		16
+#define S3		8
+#define S2		4
+#define S1		2
+volatile uint32_t s_rssi;													// 1.. >51200
+int get_sval(void)
+{
+	uint32_t s_val = s_rssi;
+	if (s_val>S940) return(94);												// Return max 2 digits!
+	if (s_val>S930) return(93);
+	if (s_val>S920) return(92);
+	if (s_val>S910) return(91);
+	if (s_val>S9)   return(9);
+	if (s_val>S8)   return(8);
+	if (s_val>S7)   return(7);
+	if (s_val>S6)   return(6);
+	if (s_val>S5)   return(5);
+	if (s_val>S4)   return(4);
+	if (s_val>S3)   return(3);
+	if (s_val>S2)   return(2);
+	return(1);
+}
+
 /*
  * AGC reference level is log2(0x40) = 6, where 0x40 is the MSB of half DAC_RANGE
  * 1/AGC_DECAY and 1/AGC_ATTACK are multipliers before agc_gain value integrator
@@ -71,6 +112,7 @@ void dsp_setmode(int mode)
  * So when delta is 1, and attack is 64, the time is 64/15625 = 4msec (fast attack)
  * The decay time is about 100x this value
  * Slow attack would be about 4096
+
  */
 #define AGC_REF		6
 #define AGC_DECAY	8192
@@ -79,6 +121,7 @@ void dsp_setmode(int mode)
 #define AGC_DIS		32766
 volatile uint16_t agc_decay  = AGC_DIS;
 volatile uint16_t agc_attack = AGC_DIS;
+
 void dsp_setagc(int agc)
 {
 	switch(agc)
@@ -135,6 +178,7 @@ void dsp_setvox(int vox)
 #define ABS(x)		( (x)<0   ? -(x) : (x) )
  
 /*
+ * Calculation of vector length:
  * Z = alpha*max(i,q) + beta*min(i,q); 
  * alpha = 1/1, beta = 3/8 (error<6.8%)
  * alpha = 15/16, beta = 15/32 (error<6.25%)
@@ -153,12 +197,17 @@ inline int16_t mag(int16_t i, int16_t q)
 
 /* 
  * Note: A simple regression IIR single pole low pass filter could be made for anti-aliasing.
- * y(n) = (1-a)*y(n-1) + a*x(n) = y(n-1) + a*(x(n) - y(n-1))
+ *  y(n) = (1-a)*y(n-1) + a*x(n) = y(n-1) + a*(x(n) - y(n-1))
  * in this a = T / (T + R*C)  
  * Example:
  *    T is sample period (e.g. 64usec) 
  *    RC the desired RC time: T*(1-a)/a.
  *    example: a=1/256 : RC = 255*64usec = 16msec (65Hz)
+ * Alternative faster implementation with higher accuracy
+ *  y(n) = y(n-1) + (x(n) - y(n-1)>>b)
+ * Here the filtered value is maintained in higher accuracy, i.e. left shifted by b bits.
+ * Before using the value: y >> b. 
+ * Also, for RC value 1/a = 1<<b, or RC = ((1<<b)-1)*64us
  */
 
 
@@ -182,14 +231,15 @@ volatile int32_t q_sample, i_sample, a_sample;								// Latest processed sample
  * The IRQ handling is redirected to a DMA channel
  * This will transfer ADC_INT samples per channel, ADC_INT maximum is 10 (would take 60usec)
  */
-#define LSH 		8														// Shift for higher accuracy of level
-#define BSH			8														// Shift for higher accuracy of bias
+#define LSH 		8														// Shift for higher accuracy of level, also LPF
+#define ADC_LEVELS	(ADC_BIAS/2)<<LSH										// Shifted initial ADC level
+#define BSH			8														// Shift for higher accuracy of bias, also LPF
 #define ADC_BIASS	ADC_BIAS<<BSH											// Shifted initial ADC bias
 #define ADC_INT		8														// Nr of samples for integration (max 8)
 volatile int16_t  adc_sample[ADC_INT][3];									// ADC samples collection
 volatile int32_t  adc_bias[3] = {ADC_BIASS, ADC_BIASS, ADC_BIASS};			// ADC dynamic bias level
 volatile int32_t  adc_result[3];											// ADC filtered result for further processing
-volatile uint32_t adc_level[3] = {10<<LSH,10<<LSH,10<<LSH};					// Levels for ADC channels
+volatile uint32_t adc_level[3] = {ADC_LEVELS, ADC_LEVELS, ADC_LEVELS};		// Levels for ADC channels
 volatile int adccnt = 0;													// Sampling overflow indicator
 
 
@@ -251,7 +301,8 @@ bool __not_in_flash_func(dsp_callback)(repeating_timer_t *t)
 	 * Here the rate is 15625Hz
 	 */
 
-	// Get samples and correct for DC bias  
+	// Get samples and correct for DC bias
+	// RC: ((1<<BSH)-1)*64usec = 16msec
 	adc_result[0] = 0;
 	adc_result[1] = 0;
 	adc_result[2] = 0;
@@ -277,17 +328,23 @@ bool __not_in_flash_func(dsp_callback)(repeating_timer_t *t)
 
 	adc_run(true);															// Start ADC again
 
-	// Calculate and save level, left shifted by LSH
-	// a=1/1024 : RC = 1023*64usec = 65msec (15Hz)
-	adc_level[0] = (1023*adc_level[0] + (ABS(adc_result[0])<<LSH))/1024;
-	adc_level[1] = (1023*adc_level[1] + (ABS(adc_result[1])<<LSH))/1024;
-	adc_level[2] = (1023*adc_level[2] + (ABS(adc_result[2])<<LSH))/1024;
+	// Calculate and save level, value is left shifted by LSH = 8
+	// RC: ((1<<LSH)-1)*64usec = 16msec
+	adc_level[0] += (ABS(adc_result[0]))-(adc_level[0]>>LSH);
+	adc_level[1] += (ABS(adc_result[1]))-(adc_level[1]>>LSH);
+	adc_level[2] += (ABS(adc_result[2]))-(adc_level[2]>>LSH);
 
-	// Crude AGC mechanism ** TO BE IMPROVED **
+	// Derive RSSI value from RX vector length
+	// Crude AGC mechanism **TO BE IMPROVED**
 	if (!tx_enabled)	
 	{
-		temp = (MAX(adc_level[1], adc_level[0]))>>LSH;						// Max I or Q
-		rx_agc = (temp==0) ? AGC_TOP : AGC_TOP/temp;						// calculate required AGC factor
+		uint32_t i=adc_level[1],q=adc_level[0];
+		if (i>q)
+			temp = (MAX(i,((29*i/32) + (61*q/128))))>>LSH;
+		else
+			temp = (MAX(q,((29*q/32) + (61*i/128))))>>LSH;
+		s_rssi = MAX(1,temp);
+		rx_agc = AGC_TOP/s_rssi;											// calculate required AGC factor
 	}
 		
 #if DSP_FFT == 1
diff --git a/dsp.h b/dsp.h
index 81a6046..99d4912 100644
--- a/dsp.h
+++ b/dsp.h
@@ -35,6 +35,9 @@
 
 /** DSP module interface **/
 
+extern volatile uint32_t s_rssi;
+int get_sval(void);
+
 extern volatile bool tx_enabled;			// Determined by (vox_active || ptt_active)
 
 #define VOX_OFF			0
diff --git a/dsp_fft.c b/dsp_fft.c
index 8a53279..3e396d8 100644
--- a/dsp_fft.c
+++ b/dsp_fft.c
@@ -155,10 +155,10 @@ bool __not_in_flash_func(rx)(void)
 	int16_t *ip, *qp, *ap, *xip, *xqp;
 	int16_t peak;
 		
-	b = dsp_active;															// Point to Active buffer
+	b = dsp_active;															// Point to Active sample buffer
 	
 	/*** Copy saved I/Q buffers to FFT buffer ***/
-	if (++b > 2) b = 0;														// Point to Old Saved buffer
+	if (++b > 2) b = 0;														// Point to Old Saved sample buffer
 	ip = &I_buf[b][0]; xip = &XI_buf[0];
 	qp = &Q_buf[b][0]; xqp = &XQ_buf[0];
 	for (i=0; i<BUFSIZE; i++)
@@ -166,7 +166,7 @@ bool __not_in_flash_func(rx)(void)
 		*xip++ = *ip++;
 		*xqp++ = *qp++;
 	}
-	if (++b > 2) b = 0;														// Point to New Saved buffer
+	if (++b > 2) b = 0;														// Point to New Saved sample buffer
 	ip = &I_buf[b][0]; xip = &XI_buf[BUFSIZE];
 	qp = &Q_buf[b][0]; xqp = &XQ_buf[BUFSIZE];
 	for (i=0; i<BUFSIZE; i++)
@@ -244,16 +244,16 @@ bool __not_in_flash_func(rx)(void)
 
 	/*** Export FFT buffer to A ***/
 	b = dsp_active;															// Assume active buffer not changed, i.e. no overruns
-	if (++b > 2) b = 0;														// Point to oldest
-	ap = &A_buf[b][0]; xip = &XI_buf[0];
+	if (++b > 2) b = 0;														// Point to oldest (will be next for output)
+	ap = &A_buf[b][0]; xip = &XI_buf[BUFSIZE];
 	for (i=0; i<BUFSIZE; i++)
 	{
-		*ap++ = *xip++;														// Copy oldest results
+		*ap++ = *xip++;														// Copy newest results
 	}
 
 
 	/*** Scale down into DAC_RANGE! ***/	
-	peak = 64;
+	peak = 128;
 	for (i=0; i<BUFSIZE; i++)									
 	{
 		A_buf[b][i] /= peak;
diff --git a/dsp_tim.c b/dsp_tim.c
index 2663063..3332bb1 100644
--- a/dsp_tim.c
+++ b/dsp_tim.c
@@ -139,7 +139,7 @@ bool __not_in_flash_func(rx)(void)
 	 * Scale and clip output,  
 	 * Send to audio DAC output
 	 */
-	a_sample = (a_sample/32) + DAC_BIAS;									// -15dB and add bias level
+	a_sample = (a_sample/64) + DAC_BIAS;									// -18dB and add bias level
 	if (a_sample > DAC_RANGE)												// Clip to DAC range
 		a_sample = DAC_RANGE;
 	else if (a_sample<0)
diff --git a/fix_fft.c b/fix_fft.c
index 9c98d5b..e7b8175 100644
--- a/fix_fft.c
+++ b/fix_fft.c
@@ -42,7 +42,7 @@
 #include "fix_fft.h"
 
 
-/** Fixed point Sine lookup table, [-1, 1] == [-32766, 32767] **/
+/** Fixed point Sine lookup table, [-1, 1] == [-32768, 32767] **/
 int16_t Sine[3*FFT_SIZE/4] =
 {
 	    0,    201,    402,    603,    804,   1005,   1206,   1406,
@@ -217,13 +217,13 @@ static int16_t bitrev[FFT_SIZE] =
 /*
  * Assume Q(0,15) notation, 1 sign, 0 int, 15 frac bits
  */
-int16_t __not_in_flash_func(FIX_MPY)(int16_t a, int16_t b)								        // Fixed-point mpy and scaling
+int16_t __not_in_flash_func(FIX_MPY)(int16_t a, int16_t b)					// Fixed-point mpy and scaling
 {
 	int32_t c;
 
 	c = (int32_t)a * (int32_t)b;											// multiply 
 	c = c + 0x4000;															// and round up
-	c = c >>15;														        // Shift right fractional bits
+	c = c >> 15;														    // Shift right fractional bits
 	return((int16_t)c);													    // Return scaled product
 }
 
@@ -287,6 +287,7 @@ int __not_in_flash_func(fix_fft)(int16_t *fr, int16_t *fi, bool inverse)
 			j = m << (k-1);													// 0 <= j < FFT_SIZE/2
 			wr = Sine[j+FFT_SIZE/4];										// Real part, i.e. Cosine
 			wi = inverse ? Sine[j] : -Sine[j];								// Imaginary part
+			
 			if (shift) { wr = wr/2; wi = wi/2; }							// Scale factors by 1/2
 
 			for (i=m; i<FFT_SIZE; i+=(step*2))								// #cycles: FFT_SIZE/step
@@ -294,10 +295,8 @@ int __not_in_flash_func(fix_fft)(int16_t *fr, int16_t *fi, bool inverse)
 				j = i + step;                                               // re-assign j !
 				tr = FIX_MPY(wr,fr[j]) - FIX_MPY(wi,fi[j]);					// Complex multiply
 				ti = FIX_MPY(wr,fi[j]) + FIX_MPY(wi,fr[j]);
-				if (shift)
-                    { qr = fr[i]/2; qi = fi[i]/2; }
-                else
-                    { qr = fr[i]; qi = fi[i]; }
+				qr = shift ? fr[i]/2 : fr[i]; 
+				qi = shift ? fi[i]/2 : fi[i]; 
 				fr[i] = qr + tr;
 				fi[i] = qi + ti;
 				fr[j] = qr - tr;
diff --git a/hmi.c b/hmi.c
index c992b82..88b8efe 100644
--- a/hmi.c
+++ b/hmi.c
@@ -110,7 +110,7 @@ char hmi_noption[HMI_NSTATES] = {HMI_NTUNE, HMI_NMODE, HMI_NAGC, HMI_NPRE, HMI_N
 char hmi_o_menu[HMI_NSTATES][8] = {"Tune","Mode","AGC","Pre","VOX"};		// Indexed by hmi_state
 char hmi_o_mode[HMI_NMODE][8] = {"USB","LSB","AM ","CW "};					// Indexed by hmi_sub[HMI_S_MODE]
 char hmi_o_agc [HMI_NAGC][8] = {"NoGC","Slow","Fast"};						// Indexed by hmi_sub[HMI_S_AGC]
-char hmi_o_pre [HMI_NPRE][8] = {"-30dB","-20dB","-10dB","0dB","+10dB"};		// Indexed by hmi_sub[HMI_S_PRE]
+char hmi_o_pre [HMI_NPRE][8] = {"-30dB","-20dB","-10dB","  0dB","+10dB"};	// Indexed by hmi_sub[HMI_S_PRE]
 char hmi_o_vox [HMI_NVOX][8] = {"NoVOX","VOX-L","VOX-M","VOX-H"};			// Indexed by hmi_sub[HMI_S_VOX]
 char hmi_o_bpf [HMI_NBPF][8] = {"<2.5","2-6","5-12","10-24","20-40"};		// Indexed by 
 
@@ -314,7 +314,11 @@ void hmi_evaluate(void)
 	char s[32];
 	
 	// Print top line of display
-	sprintf(s, "%s %7.1f %c%3d", hmi_o_mode[hmi_sub[HMI_S_MODE]], (double)hmi_freq/1000.0, (tx_enabled?0x07:0x06), (tx_enabled?0:920));
+	if (tx_enabled)
+		sprintf(s, "%s %7.1f %c %-2d", hmi_o_mode[hmi_sub[HMI_S_MODE]], (double)hmi_freq/1000.0, 0x07, 0);
+	else
+		sprintf(s, "%s %7.1f %cS%-2d", hmi_o_mode[hmi_sub[HMI_S_MODE]], (double)hmi_freq/1000.0, 0x06, get_sval());
+
 	lcd_writexy(0,0,s);
 	
 	// Print bottom line of dsiplay, depending on state
diff --git a/monitor.c b/monitor.c
index 2e1f052..e392759 100644
--- a/monitor.c
+++ b/monitor.c
@@ -177,16 +177,13 @@ void mon_or(void)
 /* 
  * ADC and AGC levels 
  */
-extern volatile uint32_t adc_level[3];	
 extern volatile int32_t  rx_agc;
 extern volatile int adccnt;
 void mon_adc(void)
 {
 	// Print results
-	printf("ADC0: %5u/2048\n", adc_level[0]>>8);
-	printf("ADC1: %5u/2048\n", adc_level[1]>>8);
-	printf("ADC2: %5u/2048\n", adc_level[2]>>8);
-	printf("AGC : %7d\n", rx_agc);
+	printf("RSSI: %5u\n", s_rssi);
+	printf("AGC : %5d\n", rx_agc);
 	printf("ADCc: %5d\n", adccnt);
 }