diff --git a/dsp.c b/dsp.c
index df5d6b8..e189719 100644
--- a/dsp.c
+++ b/dsp.c
@@ -113,6 +113,7 @@ bool rx(void)
 		
 	/*
 	 * Remove DC and store new sample
+	 //Other algo: y[i] = alpha * x[i] + (1-alpha) * y[i-1]
 	 *  w(t) = x(t) + a*w(t-1) (use a=7/8, ca 0.87)
 	 *  y(t) = w(t) - w(t-1) 
 	 */
@@ -155,12 +156,31 @@ bool rx(void)
 	
 	/* 
 	 * SSB demodulate: I[7] - Qh
-	 * Range should be within DAC_RANGE
-	 * Add 250 offset and send to audio DAC output
+	 * Add DAC_BIAS offset, Clip to DAC_RANGE and send to audio DAC output
 	 */
-	q_sample = (i_s[7] - qh)/8;
-	pwm_set_chan_level(dac_audio, PWM_CHAN_A, DAC_BIAS + q_sample);
+	q_sample = ((i_s[7] - qh)/8)+DAC_BIAS;
+	q_sample = (q_sample>DAC_RANGE)?DAC_RANGE:((q_sample<0)?0:q_sample);
+	pwm_set_chan_level(dac_audio, PWM_CHAN_A, q_sample);
 
+
+/* AGC ALGORITHM
+if(m_Peak>m_AttackAve) 																//if power is rising (use m_AttackRiseAlpha time constant)
+	m_AttackAve = (1.0-m_AttackRiseAlpha)*m_AttackAve + m_AttackRiseAlpha*m_Peak;
+else 																				//else magnitude is falling (use m_AttackFallAlpha time constant)
+	m_AttackAve = (1.0-m_AttackFallAlpha)*m_AttackAve + m_AttackFallAlpha*m_Peak;
+if(m_Peak>m_DecayAve) 																//if magnitude is rising (use m_DecayRiseAlpha time constant)
+{
+	m_DecayAve = (1.0-m_DecayRiseAlpha)*m_DecayAve + m_DecayRiseAlpha*m_Peak;
+	m_HangTimer = 0; 																//reset hang timer
+}
+else
+{ 																					//here if decreasing signal
+if(m_HangTimer<m_HangTime)
+	m_HangTimer++; 																	//just inc and hold current m_DecayAve
+else 																				//else decay with m_DecayFallAlpha which is RELEASE_TIMECONST
+	m_DecayAve = (1.0-m_DecayFallAlpha)*m_DecayAve + m_DecayFallAlpha*m_Peak;
+}
+*/
 	return true;
 }
 
diff --git a/hmi.c b/hmi.c
index 8763c72..f088d11 100644
--- a/hmi.c
+++ b/hmi.c
@@ -15,8 +15,15 @@
  *
  * The rotary encoder (GP2, GP3) controls an up/down counter connected to some field. 
  * It may be that the encoder has a bushbutton as well, this can be connected to GP4.
+ *     ___     ___
+ * ___|   |___|   |___  A
+ *   ___     ___     _
+ * _|   |___|   |___|   B
  *
- * The PTT is connected to GP15 and will always be active, even when VOX is used.
+ * Encoder channel A triggers on falling edge. 
+ * Depending on B level, count is incremented or decremented.
+ * 
+ * The PTT is connected to GP15 and will be active, except when VOX is used.
  *
  */
 #include <stdio.h>
@@ -36,10 +43,10 @@
  */
 #define GP_ENC_A	2
 #define GP_ENC_B	3
-#define GP_AUX_0	6
-#define GP_AUX_1	7
-#define GP_AUX_2	8
-#define GP_AUX_3	9
+#define GP_AUX_0	6								// Enter, Confirm
+#define GP_AUX_1	7								// Escape, Cancel
+#define GP_AUX_2	8								// Left move
+#define GP_AUX_3	9								// Right move
 #define GP_PTT		15
 #define GP_MASK_IN	((1<<GP_ENC_A)|(1<<GP_ENC_B)|(1<<GP_AUX_0)|(1<<GP_AUX_1)|(1<<GP_AUX_2)|(1<<GP_AUX_3)|(1<<GP_PTT))
 
@@ -51,22 +58,81 @@
 
 /*
  * Display layout: +----------------+
- *                 |USB 14074.1  920| --> USB mode, 14074.1 kHz, S9+20dB
- *                 |Fast  -20      5| --> Fast AGC, -20dB preamp, volume level 5
+ *                 |USB 14074.0  920| --> USB mode, 14074.0 kHz, S9+20dB
+ *                 |Tune  Att   Fast| --> Menu:Tune, Attenuator, Fast AGC
  *                 +----------------+
- * Menu statemachine: array of states and what to do per input event:
- * Menu								Encoder 	Enter		Escape		Left		Right
- * 0: Tune		Frequency			<value>		Menu 1					<dig		dig>
- * 1: Mode		USB, LSB, AM, CW	Menu 2
- * 2: AGC		Fast, Slow, Off		Menu 3
- * 3: Pre		+20dB, 0, -20dB		Menu 4
- * 4: Vol 		5, 4, 3, 2, 1
+ * LEFT and RIGHT buttons (or encoder) are used to navigate sub-menus such as {tune,mode,agc,pre}.
+ * ENTER is used to get into the sub-menu.
+ * ENTER is used again to exit and accept changes or ESCAPE to exit without changes. 
+ *
+ * When entered in a submenu:
+ * Menu		Values				Encoder 	Enter		Escape		Left		Right
+ * -------------------------------------------------------------------------------------
+ * Mode		USB, LSB, AM, CW	<value>		Accept		Exit		<value>		<value>
+ * Tune		Frequency (digit)	<value>		Accept		Exit		<=dig		dig=>
+ * AGC		Fast, Slow, Off		<value>		Accept		Exit		<value>		<value>
+ * Pre		+20dB, 0, -20dB		<value>		Accept		Exit		<value>		<value>
  */
+ 
+/* State definitions */
+#define HMI_S_MENU		0
+#define HMI_S_TUNE		1
+#define HMI_S_MODE		2
+#define HMI_S_AGC		3
+#define HMI_S_PRE		4
 
-uint8_t hmi_agc;
-uint8_t hmi_mode;
-uint8_t hmi_preamp;
-uint8_t hmi_vol;
+/* Sub menu string sets */
+char hmi_s_menu[5][8] = {"Menu","Tune","Mode","AGC ","Pre "};
+char hmi_s_mode[4][8] = {"USB", "LSB", " AM", " CW"};
+char hmi_s_agc [3][8] = {"NoGC", "Slow", "Fast"};
+char hmi_s_pre [3][8] = {"Off", "Amp", "Att"};
+
+uint8_t  hmi_state, hmi_mode, hmi_agc, hmi_pre;
+uint32_t hmi_freq;
+uint8_t  hmi_sub, hmi_option;
+
+/*
+ * Redraw the LCD
+ */
+void hmi_evaluate(void)
+{
+	char s[20];
+	
+	sprintf(s, "%s %7.1f  %3d", hmi_s_mode[hmi_mode], (double)hmi_freq/1000.0, 920);
+	lcd_writexy(0,0,s);
+	switch (hmi_state)
+	{
+	case HMI_S_MENU:
+		sprintf(s, "%s   %s  %s", hmi_s_menu[hmi_sub], hmi_s_pre[hmi_pre], hmi_s_agc[hmi_agc]);
+		lcd_writexy(0,1,s);	
+		lcd_curxy(0, 1, true);
+		break;
+	case HMI_S_TUNE:
+		sprintf(s, "%s   %s  %s", hmi_s_menu[HMI_S_TUNE], hmi_s_pre[hmi_pre], hmi_s_agc[hmi_agc]);
+		lcd_writexy(0,1,s);	
+		lcd_curxy(4+(hmi_option>4?6:hmi_option), 0, true);
+		break;
+	case HMI_S_MODE:
+		sprintf(s, "%s: %s       ", hmi_s_menu[HMI_S_MODE], hmi_s_mode[hmi_option]);
+		lcd_writexy(0,1,s);	
+		lcd_curxy(6, 1, true);
+		break;
+	case HMI_S_AGC:
+		sprintf(s, "%s: %s      ", hmi_s_menu[HMI_S_AGC], hmi_s_agc[hmi_option]);
+		lcd_writexy(0,1,s);	
+		lcd_curxy(6, 1, true);
+		break;
+	case HMI_S_PRE:
+		sprintf(s, "%s: %s       ", hmi_s_menu[HMI_S_PRE], hmi_s_pre[hmi_option]);
+		lcd_writexy(0,1,s);	
+		lcd_curxy(6, 1, true);
+		break;
+	default:
+		break;
+	}
+	
+
+}
 
 
 /*
@@ -85,15 +151,15 @@ void hmi_callback(uint gpio, uint32_t events)
 		break;
 	case GP_ENC_B:
 		break;
-	case GP_AUX_0:
+	case GP_AUX_0:									// Enter
 		break;
-	case GP_AUX_1:
+	case GP_AUX_1:									// Escape
 		break;
-	case GP_AUX_2:
+	case GP_AUX_2:									// Previous
 		break;
-	case GP_AUX_3:
+	case GP_AUX_3:									// Next
 		break;
-	case GP_PTT:
+	case GP_PTT:									// PTT
 		if (events&GPIO_IRQ_EDGE_FALL)
 			dsp_ptt(true);
 		else
@@ -103,7 +169,6 @@ void hmi_callback(uint gpio, uint32_t events)
 	default:
 		break;
 	}
-	
 }
 
 
@@ -137,14 +202,21 @@ void hmi_init(void)
 
 	// Set callback, one for all GPIO, not sure about correctness!
 	gpio_set_irq_enabled_with_callback(GP_ENC_A, GPIO_IRQ_EDGE_ALL, true, hmi_callback);
-	
+		
 	// Initialize LCD and set VFO
-	lcd_ctrl(LCD_CLEAR, 0, 0);
-	lcd_write("USB  7074.0  920");
-	lcd_ctrl(LCD_GOTO, 0, 1);
-	lcd_write("Fast  -20      5");
-	SI_SETFREQ(0, 2*7074000UL);						// Set freq to 2*7074 kHz
-	SI_SETPHASE(0, 2);								// Set phase to 180deg
+	lcd_clear();
+	
+	hmi_state = HMI_S_TUNE;
+	hmi_sub = 1;
+	hmi_option = 1;
+	hmi_mode = 0;
+	hmi_agc = 0;
+	hmi_pre = 0;
+	hmi_freq = 7074000UL;
+	
+	hmi_evaluate();
 
+	SI_SETFREQ(0, 2*hmi_freq);						// Set freq to 2*7074 kHz
+	SI_SETPHASE(0, 2);								// Set phase to 180deg
 }
 
diff --git a/hmi.h b/hmi.h
index 06f5651..8aeccb5 100644
--- a/hmi.h
+++ b/hmi.h
@@ -10,5 +10,6 @@
  */
 
 void hmi_init(void);
+void hmi_evaluate(void);
 
 #endif
diff --git a/lcd.c b/lcd.c
index 44384b3..a4334ee 100644
--- a/lcd.c
+++ b/lcd.c
@@ -8,6 +8,25 @@
  * Display RAM addresses 0x00-0x1f for top row and 0x40-0x5f for bottom row
  * Character Generator addresses are 0x00-0x07
  *
+ * Character 0x00:
+ *         +-+-+-+-+-+
+ * 000000  | |1| | | |
+ *         +-+-+-+-+-+
+ * 000001  |1| | | | |
+ *         +-+-+-+-+-+
+ * 000010  | |1| | | |
+ *         +-+-+-+-+-+
+ * 000011  |1| | | | |
+ *         +-+-+-+-+-+
+ * 000100  | |1| | | |
+ *         +-+-+-+-+-+
+ * 000101  |1| | | | |
+ *         +-+-+-+-+-+
+ * 000110  | |1| | | |
+ *         +-+-+-+-+-+
+ * 000111  | | | | | |	<= do not use, cursor 
+ *         +-+-+-+-+-+
+ *
  */
 #include <stdio.h>
 #include <string.h>
@@ -22,7 +41,7 @@
 
 
 // commands
-#define LCD_CLEARDISPLAY 	0x01
+#define LCD_CLEARDISPLAY 	0x01					// Note: LCD_ENTRYINC is set
 #define LCD_RETURNHOME 		0x02
 #define LCD_ENTRYMODESET 	0x04
 #define LCD_DISPLAYCONTROL 	0x08
@@ -32,10 +51,10 @@
 #define LCD_SETDDRAMADDR 	0x80
 
 // flags for display entry mode: LCD_ENTRYMODESET
-#define LCD_ENTRYRIGHT		0x00
-#define LCD_ENTRYLEFT		0x02
-#define LCD_ENTRYSHIFTINC	0x01
-#define LCD_ENTRYSHIFTDEC	0x00
+#define LCD_ENTRYSHIFT		0x01
+#define LCD_ENTRYNOSHIFT	0x00
+#define LCD_ENTRYINC		0x02					// Also applies to CGRAM writes
+#define LCD_ENTRYDEC		0x00					// Also applies to CGRAM writes
 
 // flags for display on/off control: LCD_DISPLAYCONTROL
 #define LCD_DISPLAYON 		0x04
@@ -59,19 +78,34 @@
 #define LCD_5x10DOTS 		0x04
 #define LCD_5x8DOTS 		0x00
 
+#define LCD_DELAY			100
+#define LCD_COMMAND			0x80
+#define LCD_DATA			0x40
 
 /* I2C address and pins */
-#define I2C_LCD 0x3E
-#define I2C0_SDA 16
-#define I2C0_SCL 17
+#define I2C_LCD 		0x3E
+#define I2C0_SDA 		16
+#define I2C0_SCL 		17
+
+
+uint8_t cgram[65] = 								// Write CGRAM
+{ 													// 8x8 bytes
+0x80,
+0x08, 0x10, 0x08, 0x10, 0x08, 0x10, 0x08, 0x00,
+0x08, 0x10, 0x08, 0x10, 0x08, 0x10, 0x0b, 0x00, 
+0x08, 0x10, 0x08, 0x10, 0x08, 0x13, 0x0b, 0x00,
+0x08, 0x10, 0x08, 0x10, 0x0b, 0x13, 0x0b, 0x00,
+0x08, 0x10, 0x08, 0x13, 0x0b, 0x13, 0x0b, 0x00,
+0x08, 0x10, 0x0b, 0x13, 0x0b, 0x13, 0x0b, 0x00,
+0x08, 0x13, 0x0b, 0x13, 0x0b, 0x13, 0x0b, 0x00,
+0x0b, 0x13, 0x0b, 0x13, 0x0b, 0x13, 0x0b, 0x00
+};
 
-uint8_t lcd_buf[2][16];								// Buffer, y={0,1}, x={0..15}
-uint8_t lcd_x, lcd_y;
 
 void lcd_init(void)
 { 
 	uint8_t txdata[8];
-		
+	
 	/* I2C0 initialisation at 400Khz. */
 	i2c_init(i2c0, 400*1000);
 	gpio_set_function(I2C0_SDA, GPIO_FUNC_I2C);
@@ -79,89 +113,111 @@ void lcd_init(void)
 	gpio_pull_up(I2C0_SDA);
 	gpio_pull_up(I2C0_SCL);
 	
-	txdata[0] = 0x80;
+	sleep_ms(50);
+	txdata[0] = LCD_COMMAND; 
+	
+	/* Initialize function set (see datasheet fig 23)*/
 	txdata[1] = LCD_FUNCTIONSET | LCD_8BITMODE | LCD_2LINE | LCD_5x8DOTS;
 	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-	sleep_us(39);
+	sleep_us(4500);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(100);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+
+	/* Initialize display control */
+	txdata[1] = LCD_DISPLAYCONTROL | LCD_DISPLAYOFF | LCD_CURSOROFF | LCD_BLINKOFF;
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+	
+	/* Display clear */
+	txdata[1] = LCD_CLEARDISPLAY;
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(1530);
+
+	/* Initialize entry mode set */ 
+	txdata[1] = LCD_ENTRYMODESET | LCD_ENTRYINC | LCD_ENTRYNOSHIFT;
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+	
+	/* Load CGRAM */
+	txdata[1] = 0x40; 								//Set CGRAM address 0
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+	i2c_write_blocking(i2c0, I2C_LCD, cgram, 65, false);
+	sleep_us(LCD_DELAY);
+	
+	/* Initialize display control */
 	txdata[1] = LCD_DISPLAYCONTROL | LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
 	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-	sleep_us(39);
+	sleep_us(LCD_DELAY);
 }
 
-void lcd_ctrl(uint8_t cmd, uint8_t x, uint8_t y)
-{
-	uint8_t txdata[8];
-	
-	txdata[0] = 0x80;
-	switch(cmd)
-	{
-	case LCD_CLEAR:
-		for (lcd_x=0; lcd_x<16; lcd_x++)
-		{
-			lcd_buf[0][lcd_x] = ' ';
-			lcd_buf[1][lcd_x] = ' ';
-		}
-		lcd_y = 0x00;
-		lcd_x = 0x00;		
-		txdata[1] = LCD_CLEARDISPLAY;
-		i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-		sleep_us(1530);
-		break;
-	case LCD_HOME:
-		lcd_y = 0x00;
-		lcd_x = 0x00;		
-		txdata[1] = LCD_RETURNHOME;
-		i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-		sleep_us(39);
-		break;
-	case LCD_CURSOR:
-		if (x==1)
-			txdata[1] = 0x0e;
-		else
-			txdata[1] = 0x0c;
-		i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-		sleep_us(39);
-		break;
-	case LCD_GOTO: 					// 2-row is 0x00-0x27 per row, only 0x00-0x1F are visible
-		lcd_y = y&0x01;
-		lcd_x = x&0x0f;
-		if (y==1)
-			txdata[1] = lcd_x | 0xc0;
-		else
-			txdata[1] = lcd_x | 0x80;
-		i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-		sleep_us(39);
-		break;
-	}
-}
 
-void lcd_put(uint8_t c)
+void lcd_clear(void)
 {
 	uint8_t txdata[3];
 
-	lcd_buf[lcd_y][lcd_x]=c;
-	lcd_x = (lcd_x<15)?(lcd_x + 1):lcd_x;
-	txdata[0] = 0x40;
-	txdata[1] = c;
+	txdata[0] = LCD_COMMAND; 
+	txdata[1] = LCD_CLEARDISPLAY;
 	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
-	sleep_us(43);
+	sleep_us(1530);
 }
 
-void lcd_write(uint8_t *s)
+void lcd_curxy(uint8_t x, uint8_t y, bool on)
+{
+	uint8_t txdata[3];
+
+	x &= 0x0f;
+	y &= 0x01;
+	txdata[0] = LCD_COMMAND; 
+	txdata[1] = x | 0x80 | (y==1?0x40:0x00);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+	
+	txdata[0] = LCD_COMMAND; 
+	txdata[1] = LCD_DISPLAYCONTROL | LCD_DISPLAYON | (on?LCD_CURSORON:LCD_CURSOROFF) | LCD_BLINKOFF;
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+}
+
+void lcd_putxy(uint8_t x, uint8_t y, uint8_t c)
+{
+	uint8_t txdata[3];
+
+	x &= 0x0f;
+	y &= 0x01;
+	txdata[0] = LCD_COMMAND; 
+	txdata[1] = x | 0x80 | (y==1?0x40:0x00);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+
+	txdata[0] = LCD_DATA; 
+	txdata[1] = c;
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+}
+
+void lcd_writexy(uint8_t x, uint8_t y, uint8_t *s)
 {
 	uint8_t i, len;
 	uint8_t txdata[18];
 
+	x &= 0x0f;
+	y &= 0x01;
+	txdata[0] = LCD_COMMAND; 
+	txdata[1] = x | 0x80 | ((y==1)?0x40:0x00);
+	i2c_write_blocking(i2c0, I2C_LCD, txdata, 2, false);
+	sleep_us(LCD_DELAY);
+
 	len = strlen(s);
-	len = (len>(16-lcd_x))?(16-lcd_x):len;
-	txdata[0] = 0x40;
-	for(i=0; i<len; i++)
-	{
-		lcd_buf[lcd_y][lcd_x++]=s[i];
-		txdata[i+1]=s[i];
-	}
+	len = (len>(16-x))?(16-x):len;
+	txdata[0] = LCD_DATA; 
+	for(i=0; i<len; i++) txdata[i+1]=s[i];
 	i2c_write_blocking(i2c0, I2C_LCD, txdata, len+1, false);
-	sleep_us(43);
+	sleep_us(LCD_DELAY);
 }
 
 
@@ -171,15 +227,13 @@ void lcd_test(void)
 	int i, j;
 	
 	chr[16] = 0; 
-	lcd_ctrl(LCD_CLEAR,0,0);
+	lcd_clear();
 	for (i=0; i<16; i++)
 	{
 		for(j=0; j<16; j++) chr[j] = (uint8_t)(16*i+j);
-		lcd_ctrl(LCD_GOTO,0,0);
-		lcd_write(chr);
+		lcd_writexy(0, 0, chr);
 		sleep_ms(800);
-		lcd_ctrl(LCD_GOTO,0,1);
-		lcd_write(chr);
+		lcd_writexy(0, 1, chr);
 	}
-	lcd_ctrl(LCD_CLEAR,0,0);
+	lcd_clear();
 }
\ No newline at end of file
diff --git a/lcd.h b/lcd.h
index 77ae3b0..2955860 100644
--- a/lcd.h
+++ b/lcd.h
@@ -12,17 +12,11 @@
 #include "pico/stdlib.h"
 #include "hardware/i2c.h"
 
-#define LCD_CLEAR		0
-#define LCD_HOME		1
-#define LCD_BLINK		2
-#define LCD_GOTO		3
-#define LCD_CURSOR		4
-
 void lcd_init(void);
-void lcd_ctrl(uint8_t cmd, uint8_t x, uint8_t y);
-void lcd_put(uint8_t c);
-void lcd_write(uint8_t *s);
+void lcd_clear(void);
+void lcd_curxy(uint8_t x, uint8_t y, bool on);
+void lcd_putxy(uint8_t x, uint8_t y, uint8_t c);
+void lcd_writexy(uint8_t x, uint8_t y, uint8_t *s);
 void lcd_test(void);
 
-
 #endif
diff --git a/monitor.c b/monitor.c
index 3c6e7bb..167e66f 100644
--- a/monitor.c
+++ b/monitor.c
@@ -26,10 +26,10 @@
 
 char mon_cmd[CMD_LEN+1];
 
-
 uint8_t si5351_reg[200];
 bool ptt = false;
 
+
 /* Commandstring parser */
 char delim[] = " ";
 #define NCMD	4
@@ -79,30 +79,36 @@ void mon_parse(char* s)
 
 void mon_init()
 {
-	/* Initialize IOs */
-    stdio_init_all();
+    stdio_init_all();								// Initialize Standard IO
+	mon_cmd[CMD_LEN] = '\0';						// Termination to be sure
 }
 
-void mon_read(uint32_t timeout)
+/*
+ * This function collects characters from stdin until CR or LF
+ * Then the command is send to a parser and executed.
+ */
+void mon_evaluate(uint32_t timeout)
 {
 	static int i = 0;
-	int c = getchar_timeout_us(timeout);
+	int c = getchar_timeout_us(timeout);			// This is the only SDK way to read from stdin
+	if (c==PICO_ERROR_TIMEOUT) return;				// Early bail out
+	
 	switch (c)
 	{
-	case PICO_ERROR_TIMEOUT:						// just go-on
-		break;
-	case CR:										// CR or LF:
-	case LF:										// 		need to parse command string
-		putchar((char)c);							// echo character
-		if (i==0) break;							// already did a parse, only do it once
-		mon_cmd[i] = '\0';							// terminate command string
+	case CR:										// CR : need to parse command string
+		putchar((char)c);							// Echo character
+		mon_cmd[i] = '\0';							// Terminate command string		
+		if (i>0)									// something to parse?
+			mon_parse(mon_cmd);						// process command
 		i=0;										// reset index
-		mon_parse(mon_cmd);							// process command
 		printf("Pico> ");							// prompt
 		break;
+	case LF:
+		putchar((char)c);							// Echo character
+		break;										// Further ignore, assume CR as terminator
 	default:
-		if ((c<32)||(c>=128)) break;				// Alfanumeric?
-		putchar((char)c);							// echo character
+		if ((c<32)||(c>=128)) break;				// Only allow alfanumeric
+		putchar((char)c);							// Echo character
 		mon_cmd[i] = (char)c;						// store in command string
 		if (i<CMD_LEN) i++;							// check range and increment
 		break;
diff --git a/monitor.h b/monitor.h
index 96bbd7d..6dacb50 100644
--- a/monitor.h
+++ b/monitor.h
@@ -10,6 +10,6 @@
  */
 
 void mon_init();
-void mon_read(uint32_t timeout);
+void mon_evaluate(uint32_t timeout);
 
 #endif
\ No newline at end of file
diff --git a/uSDR.c b/uSDR.c
index e4221d5..e3dc102 100644
--- a/uSDR.c
+++ b/uSDR.c
@@ -56,21 +56,11 @@ int main()
 	lcd_init();										// LCD output unit
 	hmi_init();										// HMI user inputs
 	
-	//SI_SETFREQ(0, 2*7074000UL);						// Set freq to 2*7074 kHz
-	//SI_SETPHASE(0, 2);								// Set phase to 180deg
-	//si_evaluate();									// Commit setting
-
-	//lcd_test();									// Test LCD character set
-	
-	//lcd_ctrl(LCD_GOTO, 0, 0);						// Go to (col, row)
-	//lcd_write(" 7074.0 kHz  USB");					// Max 16 char per line!
-	//lcd_ctrl(LCD_GOTO, 1, 0);
-	//lcd_ctrl(LCD_CURSOR, 1, 0);						// Switch cursor on
-	
 	while (1) 
 	{
-		mon_read(100000L);							// Check monitor input, wait max 100msec
-		si_evaluate();								// Check VFO settings
+		mon_evaluate(10000L);						// Check monitor input, wait max 10000 usec
+		si_evaluate();								// Refresh VFO settings
+		hmi_evaluate();								// Refresh HMI
 	}
 
     return 0;