diff --git a/nanovna.h b/nanovna.h
index d47c590..434cce7 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -829,6 +829,8 @@ void enter_dfu(void);
 /*
  * adc.c
  */
+#define ADC_TOUCH_X  ADC_CHSELR_CHSEL6
+#define ADC_TOUCH_Y  ADC_CHSELR_CHSEL7
 
 void adc_init(void);
 uint16_t adc_single_read(uint32_t chsel);
diff --git a/sa_cmd.c b/sa_cmd.c
index 690f02e..489eaca 100644
--- a/sa_cmd.c
+++ b/sa_cmd.c
@@ -270,7 +270,7 @@ VNA_SHELL_FUNCTION(cmd_v)
         shell_printf("%d\r\n", SI4432_Sel);
         return;
     }
-    VFO = my_atoi(argv[0]);
+    VFO = my_atoi(argv[0]) > 0 ? 1 : 0;
     shell_printf("VFO %d\r\n", VFO);
 }
 
diff --git a/sa_core.c b/sa_core.c
index acfb67a..953a2ec 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -857,8 +857,8 @@ uint32_t peakFreq;
 int peakIndex;
 float temppeakLevel;
 int temppeakIndex;
-static unsigned long old_freq[4] = { 0, 0, 0, 0 };
-static unsigned long real_old_freq[4] = { 0, 0, 0, 0 };
+static unsigned long old_freq[4] = { 0, 0, 0, 0};
+static unsigned long real_old_freq[4] = { 0, 0, 0, 0};
 // volatile int t;
 
 //static uint32_t extra_vbw_step_time = 0;
@@ -1785,7 +1785,7 @@ sweep_again:                                // stay in sweep loop when output mo
       if (setting.trigger == T_SINGLE)
         pause_sweep();                    // Stop scanning after completing this sweep if above trigger
     }
-    scandirty = true;                // To show trigger happened
+//    scandirty = true;                // To show trigger happened
   }
 
   // ---------------------- process measured actual sweep time -----------------
@@ -1847,8 +1847,7 @@ sweep_again:                                // stay in sweep loop when output mo
 
   if (!in_selftest && setting.mode == M_LOW && setting.auto_attenuation && max_index[0] > 0) {  // calculate and apply auto attenuate
     setting.atten_step = false;     // No step attenuate in low mode auto attenuate
-    float old_attenuate = get_attenuation();
-    float actual_max_level = actual_t[max_index[0]] - old_attenuate;
+    float actual_max_level = actual_t[max_index[0]] - get_attenuation();
     if (actual_max_level < - 31 && setting.attenuate >= 10) {
       setting.attenuate -= 10.0;
     } else if (actual_max_level < - 26 && setting.attenuate >= 5) {
@@ -1856,9 +1855,9 @@ sweep_again:                                // stay in sweep loop when output mo
     } else if (actual_max_level > - 19 && setting.attenuate <= 20) {
       setting.attenuate += 10.0;
     }
-    if (old_attenuate != get_attenuation()) {
+    // Try update settings
+    if (PE4302_Write_Byte((int) get_attenuation() * 2)) {
       redraw_request |= REDRAW_CAL_STATUS;
-      PE4302_Write_Byte((int)(setting.attenuate * 2));
       SI4432_Sel = 0;
       if (setting.atten_step) {
         set_switch_transmit();          // This should never happen
diff --git a/si4432.c b/si4432.c
index 20c4d3c..91f8f83 100644
--- a/si4432.c
+++ b/si4432.c
@@ -124,7 +124,8 @@ static void shiftOutBuf(uint8_t *buf, uint16_t size) {
   }while(--size);
 }
 #endif
-const int SI_nSEL[3] = { GPIO_RX_SEL, GPIO_LO_SEL, 0}; // #3 is dummy!!!!!!
+
+const uint16_t SI_nSEL[MAX_SI4432] = { GPIO_RX_SEL, GPIO_LO_SEL, 0}; // #3 is dummy!!!!!!
 
 volatile int SI4432_Sel = 0;         // currently selected SI4432
 // volatile int SI4432_guard = 0;
@@ -398,7 +399,7 @@ int SI4432_is_fast_mode(void)
 void SI4432_Fill(int s, int start)
 {
   SI4432_Sel = s;
-  int sel = SI_nSEL[SI4432_Sel];
+  uint16_t sel = SI_nSEL[SI4432_Sel];
 #if 0
   uint32_t t = calc_min_sweep_time_us(); // Time to delay in uS for all sweep
   if (t < setting.sweep_time_us){
@@ -657,8 +658,12 @@ void PE4302_shiftOut(uint8_t val)
      }
 }
 #endif
-void PE4302_Write_Byte(unsigned char DATA )
+
+static unsigned char old_attenuation = 0;
+bool PE4302_Write_Byte(unsigned char DATA )
 {
+  if (old_attenuation == DATA)
+    return false;
 //  chThdSleepMicroseconds(PE4302_DELAY);
   SPI2_CLK_LOW;
 //  chThdSleepMicroseconds(PE4302_DELAY);
@@ -670,7 +675,7 @@ void PE4302_Write_Byte(unsigned char DATA )
 //  chThdSleepMicroseconds(PE4302_DELAY);
   CS_PE_LOW;
 //  chThdSleepMicroseconds(PE4302_DELAY);
-
+  return true;
 }
 
 #endif
diff --git a/si4432.h b/si4432.h
index d1dba13..e01707f 100644
--- a/si4432.h
+++ b/si4432.h
@@ -21,6 +21,12 @@
 #ifndef __SI4432_H__
 
 #define __SI4432_H__
+//
+#define MAX_SI4432                         3
+
+#define SI4432_RX                          0
+#define SI4432_LO_OUT                      1
+#define SI4432_DUMMY                       2
 
 #define SI4432_DEV_TYPE                    0x00
 #define SI4432_DEV_VERSION                 0x01
@@ -112,7 +118,7 @@ void SI4432_Set_Frequency ( uint32_t Freq );
 void SI4432_Transmit(int d);
 void SI4432_Receive(void);
 uint16_t SI4432_SET_RBW(uint16_t WISH);
-void PE4302_Write_Byte(unsigned char DATA );
+bool PE4302_Write_Byte(unsigned char DATA );
 void PE4302_init(void);
 
 #ifdef __ULTRA_SA__
diff --git a/ui.c b/ui.c
index e339e83..f78b0e4 100644
--- a/ui.c
+++ b/ui.c
@@ -195,71 +195,74 @@ static int btn_wait_release(void)
   }
 }
 
+// ADC read count for measure X and Y (better be 2^n)
+#define TOUCH_MEASURE_COUNT 8
 static int
 touch_measure_y(void)
 {
-  int v;
+  int v = 0, i = TOUCH_MEASURE_COUNT;
   // open Y line
-  palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_PULLDOWN );
-  palSetPadMode(GPIOA, 7, PAL_MODE_INPUT_PULLDOWN );
+  palSetPadMode(GPIOB, GPIOB_YN, PAL_MODE_INPUT_PULLDOWN);
+  palSetPadMode(GPIOA, GPIOA_YP, PAL_MODE_INPUT_PULLDOWN);
   // drive low to high on X line
-  palSetPadMode(GPIOB, 0, PAL_MODE_OUTPUT_PUSHPULL );
-  palClearPad(GPIOB, 0);
-  palSetPadMode(GPIOA, 6, PAL_MODE_OUTPUT_PUSHPULL );
-  palSetPad(GPIOA, 6);
+  palSetPadMode(GPIOB, GPIOB_XN, PAL_MODE_OUTPUT_PUSHPULL);
+  palSetPadMode(GPIOA, GPIOA_XP, PAL_MODE_OUTPUT_PUSHPULL);
+  // drive low to high on X line (coordinates from left to right)
+  palClearPad(GPIOB, GPIOB_XN);
+  palSetPad(GPIOA, GPIOA_XP);
 
-  chThdSleepMilliseconds(2);
-  v = adc_single_read(ADC_CHSELR_CHSEL7);
-  //chThdSleepMilliseconds(2);
-  //v += adc_single_read(ADC1, ADC_CHSELR_CHSEL7);
-  return v;
+  do{
+    v+= adc_single_read(ADC_TOUCH_Y);
+  }while(--i);
+  return v/TOUCH_MEASURE_COUNT;
 }
 
 static int
 touch_measure_x(void)
 {
-  int v;
-  // open X line
-  palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_PULLDOWN );
-  palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_PULLDOWN );
-  // drive low to high on Y line
-  palSetPadMode(GPIOB, 1, PAL_MODE_OUTPUT_PUSHPULL );
-  palSetPad(GPIOB, 1);
-  palSetPadMode(GPIOA, 7, PAL_MODE_OUTPUT_PUSHPULL );
-  palClearPad(GPIOA, 7);
+  int v = 0, i = TOUCH_MEASURE_COUNT;
+  // Set X line as input
+  palSetPadMode(GPIOB, GPIOB_XN, PAL_MODE_INPUT_PULLDOWN);
+  palSetPadMode(GPIOA, GPIOA_XP, PAL_MODE_INPUT_PULLDOWN);
+  // Set Y line as output
+  palSetPadMode(GPIOB, GPIOB_YN, PAL_MODE_OUTPUT_PUSHPULL);
+  palSetPadMode(GPIOA, GPIOA_YP, PAL_MODE_OUTPUT_PUSHPULL);
+  // drive high to low on Y line (coordinates from bottom to top)
+  palSetPad(GPIOB, GPIOB_YN);
+  palClearPad(GPIOA, GPIOA_YP);
 
-  chThdSleepMilliseconds(2);
-  v = adc_single_read(ADC_CHSELR_CHSEL6);
-  //chThdSleepMilliseconds(2);
-  //v += adc_single_read(ADC1, ADC_CHSELR_CHSEL6);
-  return v;
+  do{
+    v+= adc_single_read(ADC_TOUCH_X);
+  }while(--i);
+  return v/TOUCH_MEASURE_COUNT;
 }
 
 void
 touch_prepare_sense(void)
 {
-  // open Y line
-  palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_PULLDOWN );
-  palSetPadMode(GPIOA, 7, PAL_MODE_INPUT_PULLDOWN );
+  // Set Y line as input
+  palSetPadMode(GPIOB, GPIOB_YN, PAL_MODE_INPUT_PULLDOWN);
+  palSetPadMode(GPIOA, GPIOA_YP, PAL_MODE_INPUT_PULLDOWN);
   // force high X line
-  palSetPadMode(GPIOB, 0, PAL_MODE_OUTPUT_PUSHPULL );
-  palSetPad(GPIOB, 0);
-  palSetPadMode(GPIOA, 6, PAL_MODE_OUTPUT_PUSHPULL );
-  palSetPad(GPIOA, 6);
+  palSetPadMode(GPIOB, GPIOB_XN, PAL_MODE_OUTPUT_PUSHPULL);
+  palSetPadMode(GPIOA, GPIOA_XP, PAL_MODE_OUTPUT_PUSHPULL);
+  // drive high on X line (for touch sense on Y)
+  palSetPad(GPIOB, GPIOB_XN);
+  palSetPad(GPIOA, GPIOA_XP);
 }
 
 void
 touch_start_watchdog(void)
 {
   touch_prepare_sense();
-  adc_start_analog_watchdogd(ADC_CHSELR_CHSEL7);
+  adc_start_analog_watchdogd(ADC_TOUCH_Y);
 }
 
 static int
 touch_status(void)
 {
   touch_prepare_sense();
-  return adc_single_read(ADC_CHSELR_CHSEL7) > TOUCH_THRESHOLD;
+  return adc_single_read(ADC_TOUCH_Y) > TOUCH_THRESHOLD;
 }
 
 static int
@@ -267,7 +270,7 @@ touch_check(void)
 {
   int stat = touch_status();
   if (stat) {
-    chThdSleepMilliseconds(10);
+//    chThdSleepMilliseconds(10);
     int x = touch_measure_x();
     int y = touch_measure_y();
     if (touch_status()) {