diff --git a/main.c b/main.c
index 25d861c..b005499 100644
--- a/main.c
+++ b/main.c
@@ -128,8 +128,10 @@ static THD_FUNCTION(Thread1, arg)
   (void)arg;
   chRegSetThreadName("sweep");
 
+  bool completed = false;
+  ui_process();
+
   while (1) {
-    bool completed = false;
     if (sweep_mode&(SWEEP_ENABLE|SWEEP_ONCE)) {
 //      if (dirty)
         completed = sweep(true);
@@ -145,7 +147,8 @@ static THD_FUNCTION(Thread1, arg)
       calibrate();
       sweep_mode = SWEEP_ENABLE;
     } else {
-      __WFI();
+//      if (setting.mode != -1)
+        __WFI();
     }
     // Run Shell command in sweep thread
     if (shell_function) {
diff --git a/nanovna.h b/nanovna.h
index 12fea01..f52dd95 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -566,6 +566,7 @@ typedef struct setting
   float offset;
   float trigger_level;
   int trigger;
+  int linearity_step;
   uint32_t checksum;
 }setting_t;
 
@@ -802,7 +803,7 @@ void wait_user(void);
 void calibrate(void);
 
 enum {
-  M_OFF, M_IMD, M_OIP3, M_PHASE_NOISE, M_STOP_BAND, M_PASS_BAND
+  M_OFF, M_IMD, M_OIP3, M_PHASE_NOISE, M_STOP_BAND, M_PASS_BAND, M_LINEARITY
 };
 
 enum {
diff --git a/sa_core.c b/sa_core.c
index 82efafd..db0a184 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -49,6 +49,7 @@ void reset_settings(int m)
   setting.offset = 0.0;
   setting.trigger = T_AUTO;
   setting.trigger_level = -150.0;
+  setting.linearity_step = 0;
   trace[TRACE_STORED].enabled = false;
   trace[TRACE_TEMP].enabled = false;
 #ifdef __SPUR__
@@ -140,6 +141,14 @@ void set_noise(int d)
 void set_measurement(int m)
 {
   setting.measurement = m;
+  if (m == M_LINEARITY) {
+    trace[TRACE_STORED].enabled = true;
+    for (int j = 0; j < setting._sweep_points; j++)
+      stored_t[j] = -150;
+    setting.linearity_step = 0;
+    setting.attenuate = 29;
+    setting.auto_attenuation = false;
+  }
   dirty = true;
 }
 void set_drive(int d)
@@ -498,6 +507,7 @@ void set_mode(int m)
   if (m == 6)
     m = M_ULTRA;
 #endif
+  dirty = true;
   if (setting.mode == m)
     return;
   reset_settings(m);
@@ -561,6 +571,10 @@ void set_freq(int V, unsigned long freq)
   if (old_freq[V] != freq) {
     if (V <= 1) {
       SI4432_Sel = V;
+      if (freq < 240000000 || freq > 960000000) {
+        old_freq[V] = freq + 1;
+        return;
+      }
       SI4432_Set_Frequency(freq);
 #ifdef __ULTRA_SA__
     } else {
@@ -1078,6 +1092,8 @@ static bool sweep(bool break_on_operation)
 {
   float RSSI;
   int16_t downslope;
+//  if (setting.mode== -1)
+//    return;
 //  START_PROFILE;
 again:
   downslope = true;
@@ -1325,6 +1341,13 @@ again:
     min_level = temp_min_level;
   }
   }
+  if (setting.measurement == M_LINEARITY && setting.linearity_step < setting._sweep_points) {
+    setting.attenuate = 29 - setting.linearity_step * 30 / 290;
+    dirty = true;
+    stored_t[setting.linearity_step] = peakLevel;
+    setting.linearity_step++;
+  }
+
   //    redraw_marker(peak_marker, FALSE);
 //  STOP_PROFILE;
   palSetPad(GPIOB, GPIOB_LED);
diff --git a/ui_sa.c b/ui_sa.c
index 49d1042..bfcbff7 100644
--- a/ui_sa.c
+++ b/ui_sa.c
@@ -714,6 +714,10 @@ static void menu_measure_cb(int item, uint8_t data)
       set_measurement(M_PASS_BAND);
 //      SetAverage(4);
 
+      break;
+    case M_LINEARITY:
+      set_measurement(M_LINEARITY);
+      ui_mode_normal();
       break;
   }
   kp_help_text = NULL;
@@ -1270,12 +1274,13 @@ static const menuitem_t menu_settings[] =
 };
 
 static const menuitem_t menu_measure[] = {
-  { MT_CALLBACK, M_OFF,         "OFF",      menu_measure_cb},
-  { MT_CALLBACK, M_IMD,         "MARMONICS",menu_measure_cb},
-  { MT_CALLBACK, M_OIP3,        "OIP3",     menu_measure_cb},
-  { MT_CALLBACK, M_PHASE_NOISE, "\2PHASE\0NOISE",menu_measure_cb},
-  { MT_CALLBACK, M_STOP_BAND, "\2STOP\0BAND",menu_measure_cb},
-  { MT_CALLBACK, M_PASS_BAND, "\2PASS\0BAND",menu_measure_cb},
+  { MT_CALLBACK, M_OFF,         "OFF",              menu_measure_cb},
+  { MT_CALLBACK, M_IMD,         "MARMONICS",        menu_measure_cb},
+  { MT_CALLBACK, M_OIP3,        "OIP3",             menu_measure_cb},
+  { MT_CALLBACK, M_PHASE_NOISE, "\2PHASE\0NOISE",   menu_measure_cb},
+  { MT_CALLBACK, M_STOP_BAND,   "\2STOP\0BAND",     menu_measure_cb},
+  { MT_CALLBACK, M_PASS_BAND,   "\2PASS\0BAND",     menu_measure_cb},
+  { MT_CALLBACK, M_LINEARITY,   "LINEARITY",        menu_measure_cb},
   { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
   { MT_NONE,   0, NULL, NULL } // sentinel
 };