diff --git a/main.c b/main.c
index 9005ba4..54f0913 100644
--- a/main.c
+++ b/main.c
@@ -34,11 +34,11 @@
 
 #define ENABLED_DUMP
 
-static void apply_error_term(void);
 static void apply_error_term_at(int i);
 static void apply_edelay_at(int i);
-
 static void cal_interpolate(int s);
+void update_frequencies(void);
+void set_frequencies(uint32_t start, uint32_t stop, int16_t points);
 
 static void set_frequencies(uint32_t start, uint32_t stop, int16_t points);
 static void update_frequencies(void);
@@ -49,10 +49,11 @@ bool sweep(bool break_on_operation);
 static MUTEX_DECL(mutex);
 
 #define DRIVE_STRENGTH_AUTO (-1)
-#define FREQ_HARMONICS 300000000
+#define FREQ_HARMONICS (config.harmonic_freq_threshold)
+#define IS_HARMONIC_MODE(f) ((f) > FREQ_HARMONICS)
 
 int32_t frequency_offset = 5000;
-int32_t frequency = 10000000;
+uint32_t frequency = 10000000;
 int8_t drive_strength = DRIVE_STRENGTH_AUTO;
 int8_t sweep_enabled = TRUE;
 int8_t sweep_once = FALSE;
@@ -151,7 +152,7 @@ transform_domain(void)
   // and calculate ifft for time domain
   float* tmp = (float*)spi_buffer;
 
-  uint8_t window_size = 0, offset = 0;
+  uint8_t window_size = 101, offset = 0;
   uint8_t is_lowpass = FALSE;
   switch (domain_mode & TD_FUNC) {
       case TD_FUNC_BANDPASS:
@@ -263,36 +264,35 @@ static void cmd_reset(BaseSequentialStream *chp, int argc, char *argv[])
       ;
 }
 
-int set_frequency(int freq)
+const int8_t gain_table[] = {
+  0,  // 0 ~ 300MHz
+  40, // 300 ~ 600MHz
+  50, // 600 ~ 900MHz
+  75, // 900 ~ 1200MHz
+  85, // 1200 ~ 1400MHz
+  95  // 1400MHz ~
+};
+
+static int
+adjust_gain(int newfreq)
+{
+  int delay = 0;
+  int new_order = newfreq / FREQ_HARMONICS;
+  int old_order = frequency / FREQ_HARMONICS;
+  if (new_order != old_order) {
+    tlv320aic3204_set_gain(gain_table[new_order], gain_table[new_order]);
+    delay += 10;
+  }
+  return delay;
+}
+
+int set_frequency(uint32_t freq)
 {
     int delay = 0;
     if (frequency == freq)
       return delay;
 
-    if (freq > 1400000000 && frequency <= 1400000000) {
-      tlv320aic3204_set_gain(95, 95);
-      delay += 10;
-    } else
-    if (freq > 1200000000 && frequency <= 1200000000) {
-      tlv320aic3204_set_gain(85, 85);
-      delay += 10;
-    } else
-    if (freq > 900000000 && frequency <= 900000000) {
-      tlv320aic3204_set_gain(75, 75);
-      delay += 10;
-    } else
-    if (freq > 600000000 && frequency <= 600000000) {
-      tlv320aic3204_set_gain(50, 50);
-      delay += 10;
-    } else
-    if (freq > FREQ_HARMONICS && frequency <= FREQ_HARMONICS) {
-      tlv320aic3204_set_gain(40, 40);
-      delay += 10;
-    } else
-    if (freq <= FREQ_HARMONICS && frequency > FREQ_HARMONICS) {
-      tlv320aic3204_set_gain(0, 0);
-      delay += 10;
-    }
+    delay += adjust_gain(freq);
 
     int8_t ds = drive_strength;
     if (ds == DRIVE_STRENGTH_AUTO) {
@@ -361,6 +361,18 @@ static void cmd_dac(BaseSequentialStream *chp, int argc, char *argv[])
     dacPutChannelX(&DACD2, 0, value);
 }
 
+static void cmd_threshold(BaseSequentialStream *chp, int argc, char *argv[])
+{
+    int value;
+    if (argc != 1) {
+        chprintf(chp, "usage: threshold {frequency in harmonic mode}\r\n");
+        chprintf(chp, "current: %d\r\n", config.harmonic_freq_threshold);
+        return;
+    }
+    value = atoi(argv[0]);
+    config.harmonic_freq_threshold = value;
+}
+
 static void cmd_saveconfig(BaseSequentialStream *chp, int argc, char *argv[])
 {
   (void)argc;
@@ -605,6 +617,7 @@ config_t config = {
   /* trace_colors[4] */ // { RGB565(0,255,255), RGB565(255,0,40), RGB565(0,0,255), RGB565(50,255,0) },
   /* touch_cal[4] */ { 411, 592, 151, 189 },
   /* default_loadcal */    0,
+  /* harmonic_freq_threshold */ 300000000,
   /* checksum */           0
 };
 
@@ -823,16 +836,15 @@ freq_mode_centerspan(void)
 #define STOP_MAX 1500000000
 
 void
-set_sweep_frequency(int type, float frequency)
+set_sweep_frequency(int type, int32_t freq)
 {
-  int32_t freq = frequency;
   int cal_applied = cal_status & CALSTAT_APPLY;
   switch (type) {
   case ST_START:
     freq_mode_startstop();
-    if (frequency < START_MIN)
+    if (freq < START_MIN)
       freq = START_MIN;
-    if (frequency > STOP_MAX)
+    if (freq > STOP_MAX)
       freq = STOP_MAX;
     if (frequency0 != freq) {
       ensure_edit_config();
@@ -845,9 +857,9 @@ set_sweep_frequency(int type, float frequency)
     break;
   case ST_STOP:
     freq_mode_startstop();
-    if (frequency > STOP_MAX)
+    if (freq > STOP_MAX)
       freq = STOP_MAX;
-    if (frequency < START_MIN)
+    if (freq < START_MIN)
       freq = START_MIN;
     if (frequency1 != freq) {
       ensure_edit_config();
@@ -899,7 +911,7 @@ set_sweep_frequency(int type, float frequency)
     freq_mode_centerspan();
     if (frequency0 != freq || frequency1 != 0) {
       ensure_edit_config();
-      frequency0 = frequency;
+      frequency0 = freq;
       frequency1 = 0;
       update_frequencies();
     }
@@ -1107,6 +1119,7 @@ eterm_calc_et(void)
   cal_status |= CALSTAT_ET;
 }
 
+#if 0
 void apply_error_term(void)
 {
   int i;
@@ -1138,6 +1151,7 @@ void apply_error_term(void)
     measured[1][i][1] = s21ai;
   }
 }
+#endif
 
 void apply_error_term_at(int i)
 {
@@ -1288,6 +1302,13 @@ cal_interpolate(int s)
         // found f between freqs at j and j+1
         float k1 = (float)(f - src->_frequencies[j])
                         / (src->_frequencies[j+1] - src->_frequencies[j]);
+        
+        // avoid glitch between freqs in different harmonics mode
+        if (IS_HARMONIC_MODE(src->_frequencies[j]) != IS_HARMONIC_MODE(src->_frequencies[j+1])) {
+          // assume f[j] < f[j+1]
+          k1 = IS_HARMONIC_MODE(f) ? 1.0 : 0.0;
+        }
+
         float k0 = 1.0 - k1;
         for (eterm = 0; eterm < 5; eterm++) {
           cal_data[eterm][i][0] = src->_cal_data[eterm][j][0] * k0 + src->_cal_data[eterm][j+1][0] * k1;
@@ -1990,6 +2011,7 @@ static const ShellCommand commands[] =
     { "capture", cmd_capture },
     { "vbat", cmd_vbat },
     { "transform", cmd_transform },
+    { "threshold", cmd_threshold },
     { NULL, NULL }
 };
 
@@ -2116,5 +2138,6 @@ void HardFault_Handler(void)
 
 void hard_fault_handler_c(uint32_t* sp)
 {
+  (void)sp;
   while (true) {}
 }
diff --git a/nanovna.h b/nanovna.h
index 64bd322..5ce7adb 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -71,7 +71,7 @@ enum {
   ST_START, ST_STOP, ST_CENTER, ST_SPAN, ST_CW
 };
 
-void set_sweep_frequency(int type, float frequency);
+void set_sweep_frequency(int type, int32_t frequency);
 uint32_t get_sweep_frequency(int type);
 
 float my_atof(const char *p);
@@ -106,7 +106,7 @@ void calculate_gamma(float *gamma);
 void fetch_amplitude(float *gamma);
 void fetch_amplitude_ref(float *gamma);
 
-int si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength);
+int si5351_set_frequency_with_offset(uint32_t freq, int offset, uint8_t drive_strength);
 
 
 /*
@@ -203,6 +203,7 @@ typedef struct {
   uint16_t trace_color[TRACES_MAX];
   int16_t touch_cal[4];
   int8_t default_loadcal;
+  uint32_t harmonic_freq_threshold;
   int32_t checksum;
 } config_t;
 
@@ -244,6 +245,7 @@ void redraw_marker(int marker, int update_info);
 uint16_t trace_get_info(int t, char *buf, uint16_t len);
 void plot_into_index(float measured[2][101][2]);
 void force_set_markmap(void);
+void draw_frequencies(void);
 void draw_all(bool flush);
 
 void draw_cal_status(void);
@@ -371,6 +373,8 @@ void ui_hide(void);
 
 extern uint8_t operation_requested;
 
+void touch_start_watchdog(void);
+void touch_position(int *x, int *y);
 void handle_touch_interrupt(void);
 void touch_start_watchdog(void);
 
diff --git a/plot.c b/plot.c
index a6f308a..13a3088 100644
--- a/plot.c
+++ b/plot.c
@@ -8,7 +8,6 @@
 #define SWAP(x,y) do { int z=x; x = y; y = z; } while(0)
 
 static void cell_draw_marker_info(int m, int n, int w, int h);
-void draw_frequencies(void);
 void frequency_string(char *buf, size_t len, int32_t freq);
 void markmap_all_markers(void);
 
@@ -614,7 +613,7 @@ gamma2imp(char *buf, int len, const float coeff[2], uint32_t frequency)
 }
 
 static void
-gamma2resistance(char *buf, int len, const float coeff[2], uint32_t frequency)
+gamma2resistance(char *buf, int len, const float coeff[2])
 {
   float z0 = 50;
   float d = z0 / ((1-coeff[0])*(1-coeff[0])+coeff[1]*coeff[1]);
@@ -623,7 +622,7 @@ gamma2resistance(char *buf, int len, const float coeff[2], uint32_t frequency)
 }
 
 static void
-gamma2reactance(char *buf, int len, const float coeff[2], uint32_t frequency)
+gamma2reactance(char *buf, int len, const float coeff[2])
 {
   float z0 = 50;
   float d = z0 / ((1-coeff[0])*(1-coeff[0])+coeff[1]*coeff[1]);
@@ -665,10 +664,10 @@ trace_get_value_string(int t, char *buf, int len, float coeff[2], uint32_t frequ
     chsnprintf(buf, len, "%.2fj", coeff[1]);
     break;
   case TRC_R:
-    gamma2resistance(buf, len, coeff, frequency);
+    gamma2resistance(buf, len, coeff);
     break;
   case TRC_X:
-    gamma2reactance(buf, len, coeff, frequency);
+    gamma2reactance(buf, len, coeff);
     break;
   //case TRC_ADMIT:
   case TRC_POLAR:
diff --git a/si5351.c b/si5351.c
index ebd4aa9..49de637 100644
--- a/si5351.c
+++ b/si5351.c
@@ -18,6 +18,7 @@
  * Boston, MA 02110-1301, USA.
  */
 #include "hal.h"
+#include "nanovna.h"
 #include "si5351.h"
 
 #define SI5351_I2C_ADDR   	(0x60<<1)
@@ -303,16 +304,16 @@ int current_band = -1;
  */
 #define CLK2_FREQUENCY 8000000L
 int
-si5351_set_frequency_with_offset(int freq, int offset, uint8_t drive_strength)
+si5351_set_frequency_with_offset(uint32_t freq, int offset, uint8_t drive_strength)
 {
   int band;
   int delay = 3;
   uint32_t ofreq = freq + offset;
   uint32_t rdiv = SI5351_R_DIV_1;
-  if (freq > 900000000) {
+  if (freq >= config.harmonic_freq_threshold * 3) {
     freq /= 5;
     ofreq /= 7;
-  } else if (freq > 300000000) {
+  } else if (freq >= config.harmonic_freq_threshold) {
     freq /= 3;
     ofreq /= 5;
   }
diff --git a/ui.c b/ui.c
index cc087b8..9bd595b 100644
--- a/ui.c
+++ b/ui.c
@@ -20,6 +20,7 @@
 
 #include "ch.h"
 #include "hal.h"
+#include "chprintf.h"
 #include "nanovna.h"
 #include "chprintf.h"
 #include <stdlib.h>