diff --git a/nanovna.h b/nanovna.h
index c86ea8a..f157b87 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -239,6 +239,7 @@ extern int32_t frequencyExtra;
 void set_10mhz(uint32_t);
 void set_modulation(int);
 void set_modulation_frequency(int);
+int search_maximum(int m, int center, int span);
 //extern int setting.modulation;
 void set_measurement(int);
 // extern int settingSpeed;
@@ -1003,7 +1004,7 @@ uint32_t calc_min_sweep_time_us(void);
 pureRSSI_t perform(bool b, int i, uint32_t f, int e);
 
 enum {
-  M_OFF, M_IMD, M_OIP3, M_PHASE_NOISE, M_STOP_BAND, M_PASS_BAND, M_LINEARITY, M_AM, M_FM
+  M_OFF, M_IMD, M_OIP3, M_PHASE_NOISE, M_STOP_BAND, M_PASS_BAND, M_LINEARITY, M_AM, M_FM, M_THD
 };
 
 enum {
diff --git a/plot.c b/plot.c
index 40487ae..e0c47ea 100644
--- a/plot.c
+++ b/plot.c
@@ -2081,7 +2081,49 @@ static void cell_draw_marker_info(int x0, int y0)
       active++;
     }
   }
+  if (setting.measurement == M_THD && active >= 1)
+    active = 2;
   for (int i = 0; i < MARKER_COUNT; i++) {
+    if (i == 3 && setting.measurement == M_PASS_BAND) {
+        uint32_t f;
+        if (markers[2].frequency>markers[1].frequency)
+          f = markers[2].frequency-markers[1].frequency;
+        else
+          f = markers[1].frequency-markers[2].frequency;
+        plot_printf(buf, sizeof buf, "WIDTH: %8.3qHz", f);
+        j = 3;
+        int xpos = 1 + (j%2)*(WIDTH/2) + CELLOFFSETX - x0;
+        int ypos = 1 + (j/2)*(16) - y0;
+        cell_drawstring_7x13(buf, xpos, ypos);
+//        cell_drawstring(buf, xpos, ypos);
+        break;
+    } else
+    if (i >= 2 && setting.measurement == M_THD) {
+      if (i == 2 && (markers[0].index << 5) > sweep_points ) {
+        int old_unit = setting.unit;
+        setting.unit = U_WATT;
+        float p = value((actual_t[markers[0].index]));
+        int j = 2;
+        uint32_t f = markers[0].frequency;
+        float h = 0.0;
+        while (f * j < frequencies[sweep_points-1]) {
+          if (search_maximum(1, f*j, 4*j) )             // use marker 1 for searching harmonics
+            h += value((actual_t[markers[1].index]));
+          j++;
+        }
+        float thd = 100.0 * sqrt(h/p);
+        setting.unit = old_unit;
+
+        plot_printf(buf, sizeof buf, "THD: %4.1f%%", thd);
+        j = 1;
+        int xpos = 1 + (j%2)*(WIDTH/2) + CELLOFFSETX - x0;
+        int ypos = 1 + (j/2)*(16) - y0;
+        cell_drawstring_7x13(buf, xpos, ypos);
+//        cell_drawstring(buf, xpos, ypos);
+        break;
+      }
+      break;
+    } else
     if (i >= 2 && setting.measurement == M_OIP3 && markers[2].enabled && markers[3].enabled) {
       float il = value((actual_t[markers[2].index]));
       float ir = value((actual_t[markers[3].index]));
diff --git a/sa_core.c b/sa_core.c
index f6abfd6..19df767 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -2468,14 +2468,14 @@ sweep_again:                                // stay in sweep loop when output mo
     } else if (setting.measurement == M_PASS_BAND  && markers[0].index > 10) {      // ----------------Pass band measurement
       int t = markers[0].index;
       float v = actual_t[t];
-      while (t > 0 && actual_t[t] > v - 6.0)                                        // Find left -3dB point
+      while (t > 0 && actual_t[t] > v - 4.0)                                        // Find left -3dB point
         t --;
       if (t > 0) {
         markers[1].index = t;
         markers[1].frequency = frequencies[t];
       }
       t = markers[0].index;
-      while (t < setting._sweep_points - 1 && actual_t[t] > v - 6.0)                // find right -3dB point
+      while (t < setting._sweep_points - 1 && actual_t[t] > v - 4.0)                // find right -3dB point
         t ++;
       if (t < setting._sweep_points - 1 ) {
         markers[2].index = t;
diff --git a/ui_sa.c b/ui_sa.c
index 52db604..1ebe70e 100644
--- a/ui_sa.c
+++ b/ui_sa.c
@@ -833,25 +833,26 @@ static UI_FUNCTION_ADV_CALLBACK(menu_measure_acb)
 
       break;
     case M_PASS_BAND:                             // STop band measurement
-      reset_settings(setting.mode);
+//      reset_settings(setting.mode);
+      markers[0].enabled = M_ENABLED;
+      markers[0].mtype = M_REFERENCE | M_TRACKING;
       markers[1].enabled = M_ENABLED;
       markers[1].mtype = M_DELTA;
       markers[2].enabled = M_ENABLED;
       markers[2].mtype = M_DELTA;
-      kp_help_text = "Frequency of signal";
-      ui_mode_keypad(KM_CENTER);
-      ui_process_keypad();
-      kp_help_text = "Width of signal";
-      ui_mode_keypad(KM_SPAN);
-      ui_process_keypad();
-      set_sweep_frequency(ST_SPAN, uistat.value*2);
+//      kp_help_text = "Frequency of signal";
+//      ui_mode_keypad(KM_CENTER);
+//      ui_process_keypad();
+//      kp_help_text = "Width of signal";
+//      ui_mode_keypad(KM_SPAN);
+//      ui_process_keypad();
+//      set_sweep_frequency(ST_SPAN, uistat.value*2);
       set_measurement(M_PASS_BAND);
 //      SetAverage(4);
 
       break;
     case M_LINEARITY:
       set_measurement(M_LINEARITY);
-      ui_mode_normal();
       break;
     case M_AM:                                     // OIP3
       reset_settings(setting.mode);
@@ -882,6 +883,9 @@ static UI_FUNCTION_ADV_CALLBACK(menu_measure_acb)
       set_sweep_frequency(ST_SPAN, uistat.value*30);
       set_measurement(M_FM);
       break;
+    case M_THD:
+      set_measurement(M_THD);
+      break;
   }
 #endif
 //  selection = -1;
@@ -1682,6 +1686,7 @@ static const menuitem_t menu_measure2[] = {
 //  { MT_ADV_CALLBACK | MT_LOW,   M_LINEARITY,  "LINEAR",         menu_measure_acb},
   { MT_ADV_CALLBACK,            M_AM,           "AM",           menu_measure_acb},
   { MT_ADV_CALLBACK,            M_FM,           "FM",           menu_measure_acb},
+  { MT_ADV_CALLBACK,            M_THD,          "THD",           menu_measure_acb},
   { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
   { MT_NONE,   0, NULL, NULL } // sentinel
 };
@@ -1692,7 +1697,7 @@ static const menuitem_t menu_measure[] = {
   { MT_ADV_CALLBACK,            M_OIP3,       "OIP3",           menu_measure_acb},
   { MT_ADV_CALLBACK,            M_PHASE_NOISE,"PHASE\nNOISE",   menu_measure_acb},
   { MT_ADV_CALLBACK,            M_STOP_BAND,  "SNR",            menu_measure_acb},
-  { MT_ADV_CALLBACK,            M_PASS_BAND,  "-6dB\nWIDTH",     menu_measure_acb},
+  { MT_ADV_CALLBACK,            M_PASS_BAND,  "-3dB\nWIDTH",     menu_measure_acb},
   { MT_SUBMENU,  0,             S_RARROW" MORE",                menu_measure2},
   { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
   { MT_NONE,   0, NULL, NULL } // sentinel