diff --git a/STM32F072xB.ld b/STM32F072xB.ld
index 577e87f..b1f2ea2 100644
--- a/STM32F072xB.ld
+++ b/STM32F072xB.ld
@@ -19,14 +19,14 @@
  */
 MEMORY
 {
-    flash0  : org = 0x08000000, len = 96k
+    flash0  : org = 0x08000000, len = 104k
     flash1  : org = 0x00000000, len = 0
     flash2  : org = 0x00000000, len = 0
     flash3  : org = 0x00000000, len = 0
     flash4  : org = 0x00000000, len = 0
     flash5  : org = 0x00000000, len = 0
     flash6  : org = 0x00000000, len = 0
-    flash7  : org = 0x08018000, len = 32k
+    flash7  : org = 0x0801C000, len = 16k
     ram0    : org = 0x20000000, len = 16k
     ram1    : org = 0x00000000, len = 0
     ram2    : org = 0x00000000, len = 0
diff --git a/ili9341.c b/ili9341.c
index 0cd5ad2..f423b19 100644
--- a/ili9341.c
+++ b/ili9341.c
@@ -524,7 +524,7 @@ void ili9341_clear_screen(void)
 {
   ili9341_fill(0, 0, ILI9341_WIDTH, ILI9341_HEIGHT, background_color);
 }
-
+#if 0
 void ili9341_set_foreground(uint16_t fg)
 {
   foreground_color = fg;
@@ -534,7 +534,7 @@ void ili9341_set_background(uint16_t bg)
 {
   background_color = bg; 
 }
-
+#endif
 void ili9341_set_rotation(uint8_t r)
 {
   //  static const uint8_t rotation_const[]={DISPLAY_ROTATION_0, DISPLAY_ROTATION_90,
diff --git a/main.c b/main.c
index 424b80c..51a7e6e 100644
--- a/main.c
+++ b/main.c
@@ -117,7 +117,7 @@ const char *info_about[]={
   0 // sentinel
 };
 
-static THD_WORKING_AREA(waThread1, 750);
+static THD_WORKING_AREA(waThread1, 730);
 static THD_FUNCTION(Thread1, arg)
 {
   (void)arg;
@@ -813,10 +813,10 @@ static const trace_t def_trace[TRACES_MAX] = {//enable, type, channel, reserved,
 };
 
 static const marker_t def_markers[MARKERS_MAX] = {
-    { 1, M_REFERENCE, 30, 0 },
-    { 0, M_NORMAL, 40, 0 },
-    { 0, M_NORMAL, 60, 0 },
-    { 0, M_NORMAL, 80, 0 }
+    { M_TRACKING_ENABLED,   M_REFERENCE, 30, 0 },
+    { M_DISABLED,           M_NORMAL,   40, 0 },
+    { M_DISABLED,           M_NORMAL,   60, 0 },
+    { M_DISABLED,           M_NORMAL,   80, 0 }
 };
 
 // Load propeties default settings
@@ -2140,7 +2140,7 @@ VNA_SHELL_FUNCTION(cmd_threads)
   thread_t *tp;
   (void)argc;
   (void)argv;
-  shell_printf("stklimit|   stack|stk free|    addr|refs|prio|    state|        name"VNA_SHELL_NEWLINE_STR);
+  shell_printf("stklimit|   |stk free|    addr|refs|prio|    state|        name"VNA_SHELL_NEWLINE_STR);
   tp = chRegFirstThread();
   do {
     uint32_t max_stack_use = 0U;
diff --git a/nanovna.h b/nanovna.h
index 276df10..82d5efa 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -199,8 +199,8 @@ extern int _height;
 #define CELLWIDTH  (32)
 #define CELLHEIGHT (32)
 
-//#define NGRIDY 10
-#define NGRIDY 9
+#define NGRIDY 10
+//#define NGRIDY 9
 
 #define FREQUENCIES_XPOS1 OFFSETX
 #define FREQUENCIES_XPOS2 200
@@ -316,7 +316,11 @@ float groupdelay_from_array(int i, float array[POINTS_COUNT][2]);
 #endif
 // marker
 enum {
-  M_REFERENCE, M_NORMAL, M_DELTA
+  M_REFERENCE, M_NORMAL, M_DELTA, M_TRACKING
+};
+
+enum {
+  M_DISABLED, M_ENABLED, M_TRACKING_ENABLED
 };
 
 typedef struct {
@@ -404,8 +408,13 @@ void ili9341_init(void);
 void ili9341_test(int mode);
 void ili9341_bulk(int x, int y, int w, int h);
 void ili9341_fill(int x, int y, int w, int h, int color);
+#if 0
 void ili9341_set_foreground(uint16_t fg);
 void ili9341_set_background(uint16_t fg);
+#else
+#define ili9341_set_foreground(fg) {  foreground_color = fg; }
+#define ili9341_set_background(bg) {  background_color = bg;}
+#endif
 void ili9341_clear_screen(void);
 void blit8BitWidthBitmap(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint8_t *bitmap);
 void ili9341_drawchar(uint8_t ch, int x, int y);
@@ -427,15 +436,15 @@ void show_logo(void);
 #if 1
 #define SAVEAREA_MAX 5
 // Begin addr                   0x08018000
-#define SAVE_CONFIG_AREA_SIZE   0x00008000
+#define SAVE_CONFIG_AREA_SIZE   0x00000800
 // config save area
-#define SAVE_CONFIG_ADDR        0x08018000
+#define SAVE_CONFIG_ADDR        0x0801C000
 // properties_t save area
-#define SAVE_PROP_CONFIG_0_ADDR 0x08018800
-#define SAVE_PROP_CONFIG_1_ADDR 0x0801a000
-#define SAVE_PROP_CONFIG_2_ADDR 0x0801b800
-#define SAVE_PROP_CONFIG_3_ADDR 0x0801d000
-#define SAVE_PROP_CONFIG_4_ADDR 0x0801e800
+#define SAVE_PROP_CONFIG_0_ADDR 0x0801C800
+#define SAVE_PROP_CONFIG_1_ADDR 0x0801D000
+#define SAVE_PROP_CONFIG_2_ADDR 0x0801D800
+#define SAVE_PROP_CONFIG_3_ADDR 0x0801E000
+#define SAVE_PROP_CONFIG_4_ADDR 0x0801E800
 #else
 #define SAVEAREA_MAX 4
 // Begin addr                   0x0801C000
diff --git a/plot.c b/plot.c
index 56748a3..a6e4bad 100644
--- a/plot.c
+++ b/plot.c
@@ -1465,10 +1465,10 @@ draw_all_cells(bool flush_markmap)
   if (waterfall) {
     for (m = 226; m >= HEIGHT+3; m -= 1) {		// Scroll down
       uint16_t *buf = &spi_buffer[0];
-      ili9341_read_memory(5*5, m, area_width, 1, area_width, buf);
-      ili9341_bulk(5*5,m+1, area_width,1);
+      ili9341_read_memory(5*5, m, 290, 1, 290, buf);
+      ili9341_bulk(5*5,m+1, 290,1);
     }
-    for (int i=0; i<area_width; i++) {			// Add new topline
+    for (int i=0; i<290; i++) {			// Add new topline
 #if 0
       int k = (actual_t[i]+120 + 10)* 3 / 2;
       unsigned int r=0,g=0,b=0;
@@ -1503,7 +1503,7 @@ draw_all_cells(bool flush_markmap)
 #endif
       spi_buffer[i] = RGB565(r,g,b);
     }
-    ili9341_bulk(5*5,HEIGHT+3, area_width,1);
+    ili9341_bulk(5*5,HEIGHT+3, 290,1);
   }
 #endif
 }
diff --git a/sa_core.c b/sa_core.c
index 646bf2e..3c370ea 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -458,7 +458,7 @@ void update_rbw(void)
 
 //static int spur_old_stepdelay = 0;
 static const unsigned int spur_IF =            433900000;
-static const unsigned int spur_alternate_IF =  433700000;
+static const unsigned int spur_alternate_IF =  434100000;
 static const int spur_table[] =
 {
    470000,
@@ -568,10 +568,16 @@ float perform(bool break_on_operation, int i, int32_t f, int tracking)
   return(RSSI);
 }
 
+#define MAX_MAX 4
+#define MAX_NOISE 10    //  10dB
+int16_t max_index[MAX_MAX];
+int16_t cur_max = 0;
+
 // main loop for measurement
 static bool sweep(bool break_on_operation)
 {
   float RSSI;
+  int16_t downslope = true;
   palClearPad(GPIOC, GPIOC_LED);
   temppeakLevel = -150;
   float temp_min_level = 100;
@@ -580,7 +586,6 @@ static bool sweep(bool break_on_operation)
   for (int i = 0; i < sweep_points; i++) {
     RSSI = perform(break_on_operation, i, frequencies[i], setting_tracking);
 
-//START_PROFILE
     // back to toplevel to handle ui operation
     if (operation_requested && break_on_operation)
       return false;
@@ -618,6 +623,54 @@ static bool sweep(bool break_on_operation)
       case AV_16: actual_t[i] = (actual_t[i]*3 + RSSI) / 16.0; break;
       }
     }
+#if 1
+// START_PROFILE
+    if (i == 0) {
+      cur_max = 0;          // Always at least one maximum
+      temppeakIndex = 0;
+      temppeakLevel = actual_t[i];
+      max_index[i] = 0;
+      downslope = true;
+    }
+    if (downslope) {
+      if (temppeakLevel > actual_t[i]) {    // Follow down
+        temppeakIndex = i;                  // Latest minimum
+        temppeakLevel = actual_t[i];
+      } else if (temppeakLevel + MAX_NOISE < actual_t[i]) {    // Local minimum found
+        temppeakIndex = i;                          // This is now the latest maximum
+        temppeakLevel = actual_t[i];
+        downslope = false;
+      }
+    } else {
+      if (temppeakLevel < actual_t[i]) {    // Follow up
+        temppeakIndex = i;
+        temppeakLevel = actual_t[i];
+      } else if (temppeakLevel - MAX_NOISE > actual_t[i]) {    // Local max found
+
+          int j = 0;                                            // Insertion index
+          while (j<cur_max && actual_t[max_index[j]] >= temppeakLevel)   // Find where to insert
+            j++;
+          if (j < MAX_MAX) {                                    // Larger then one of the previous found
+            int k = MAX_MAX-1;
+            while (k > j) {                                      // Shift to make room for max
+              max_index[k] = max_index[k-1];
+//              maxlevel_index[k] = maxlevel_index[k-1];        // Only for debugging
+              k--;
+            }
+            max_index[j] = temppeakIndex;
+//            maxlevel_index[j] = actual_t[temppeakIndex];      // Only for debugging
+            if (cur_max < MAX_MAX) {
+              cur_max++;
+            }
+//STOP_PROFILE
+          }
+          temppeakIndex = i;            // Latest minimum
+          temppeakLevel = actual_t[i];
+
+          downslope = true;
+      }
+    }
+#else
     if (frequencies[i] > 1000000) {
       if (temppeakLevel < actual_t[i]) {
         temppeakIndex = i;
@@ -626,7 +679,7 @@ static bool sweep(bool break_on_operation)
     }
     if (temp_min_level > actual_t[i])
       temp_min_level = actual_t[i];
-//STOP_PROFILE
+#endif
   }
 //  if (setting_spur == 1) {
 //    setting_spur = -1;
@@ -638,9 +691,37 @@ static bool sweep(bool break_on_operation)
     scandirty = false;
     draw_cal_status();
   }
-  peakIndex = temppeakIndex;
+#if 1
+  int i = 0;
+  int m = 0;
+  while (i < cur_max) {                                 // For all maxima found
+    while (m < MARKERS_MAX) {
+      if (markers[m].enabled == M_TRACKING_ENABLED) {   // Available marker found
+        markers[m].index = max_index[i];
+        markers[m].frequency = frequencies[markers[m].index];
+        m++;
+        break;                          // Next maximum
+      }
+      m++;                              // Try next marker
+    }
+    i++;
+  }
+  while (m < MARKERS_MAX) {
+    if (markers[m].enabled == M_TRACKING_ENABLED ) {    // More available markers found
+      markers[m].index = 0;                             // Enabled but no max
+      markers[m].frequency = frequencies[markers[m].index];
+    }
+    m++;                              // Try next marker
+  }
+  peakIndex = max_index[0];
   peakLevel = actual_t[peakIndex];
   peakFreq = frequencies[peakIndex];
+#else
+  int peak_marker = 0;
+  markers[peak_marker].enabled = true;
+  markers[peak_marker].index = peakIndex;
+  markers[peak_marker].frequency = frequencies[markers[peak_marker].index];
+#endif
   min_level = temp_min_level;
 #if 0                           // Auto ref level setting
   int scale = get_trace_scale(2);
@@ -655,80 +736,12 @@ static bool sweep(bool break_on_operation)
   }
 
 #endif
-  int peak_marker = 0;
-  markers[peak_marker].enabled = true;
-  markers[peak_marker].index = peakIndex;
-  markers[peak_marker].frequency = frequencies[markers[peak_marker].index];
   //    redraw_marker(peak_marker, FALSE);
   palSetPad(GPIOC, GPIOC_LED);
   return true;
 }
 
 
-#if 0
-void PeakSearch()
-{
-#define PEAKSTACK   4
-#define PEAKDISTANCE    10
-  int level = 0;
-  int searchLeft[PEAKSTACK];
-  int peakIndex[PEAKSTACK];
-  int peak_marker = 0;
-  searchLeft[level] = true;
-  peakIndex[level] = markers[peak_marker].index;
-  level++;
-  searchLeft[level] = true;
-  int peakFrom;
-  int peakTo;
-  while (peak_marker < 4){
-    if (searchLeft[level])
-    {
-      int fromLevel = level;
-      while (fromLevel > 0 && searchLeft[fromLevel])
-        fromLevel--
-      if(fromLevel == 0) {
-        peakFrom = PEAKDISTANCE;
-      } else {
-        peakFrom = peakIndex[fromLevel] + PEAKDISTANCE;
-      }
-      peakTo = peakIndex[level] - PEAKDISTANCE;
-    } else {
-      int toLevel = level;
-      while (toLevel > 0 && !searchLeft[toLevel])
-        toLevel--
-      if(toLevel == 0) {
-        peakTo = POINTS_COUNT - 1 - PEAKDISTANCE;
-      } else {
-        peakTo = peakIndex[fromLevel] - PEAKDISTANCE;
-      }
-      peakFrom = peakIndex[level] + PEAKDISTANCE;
-    }
-    float peakMax = actual_t[peakFrom];
-    int peakIndex = peakFrom;
-    for (int i = peakFrom; i < peakTo; i++) {
-      if (peakMax < actual_t[i]) {
-        peakMax = actual_t[i];
-        peakIndex = i;
-      }
-    }
-
-
-  peakIndex = temppeakIndex;
-  peakLevel = actual_t[peakIndex];
-  peakFreq = frequencies[peakIndex];
-  setting_spur = -setting_spur;
-  int peak_marker = 0;
-  markers[peak_marker].enabled = true;
-  markers[peak_marker].index = peakIndex;
-  markers[peak_marker].frequency = frequencies[markers[peak_marker].index];
-//    redraw_marker(peak_marker, FALSE);
-
-
-}
-
-}
-#endif
-
 const char *averageText[] = { "OFF", "MIN", "MAX", "2", "4", "8"};
 const char *dBText[] = { "1dB/", "2dB/", "5dB/", "10dB/", "20dB/"};
 const int refMHz[] = { 30, 15, 10, 4, 3, 2, 1 };
diff --git a/ui.c b/ui.c
index 544d47b..6fc675c 100644
--- a/ui.c
+++ b/ui.c
@@ -434,10 +434,13 @@ enum {
   MT_CALLBACK,
   MT_CANCEL,
   MT_TITLE,
-  MT_CLOSE
+  MT_CLOSE,
+  MT_KEYPAD
 };
-#define MT_FORM 0x80        // Or with menu type to get large button with current value
-#define MT_MASK(x) (0x7F & (x))
+#define MT_FORM     0x80        // Or with menu type to get large button with current value
+#define MT_BACK     0x40
+#define MT_LEAVE    0x20
+#define MT_MASK(x) (0xF & (x))
 
 typedef void (*menuaction_cb_t)(int item, uint8_t data);
 
@@ -767,15 +770,15 @@ menu_marker_op_cb(int item, uint8_t data)
 static void
 menu_marker_search_cb(int item, uint8_t data)
 {
-  (void)data;
+  (void)item;
   int i = -1;
   if (active_marker == -1)
     return;
 
-  switch (item) {
+  switch (data) {
   case 0: /* maximum */
   case 1: /* minimum */
-    set_marker_search(item);
+    set_marker_search(data);
     i = marker_search();
     break;
   case 2: /* search Left */
@@ -830,18 +833,18 @@ menu_marker_sel_cb(int item, uint8_t data)
     if (markers[item].enabled) {
       if (item == active_marker) {
         // disable if active trace is selected
-        markers[item].enabled = FALSE;
+        markers[item].enabled = M_DISABLED;
         active_marker_select(-1);
       } else {
         active_marker_select(item);
       }
     } else {
-      markers[item].enabled = TRUE;
+      markers[item].enabled = M_TRACKING_ENABLED; // default tracking enabled
       active_marker_select(item);
     }
   } else if (item == 4) { /* all off */
       for (t = 0; t < MARKERS_MAX; t++)
-        markers[t].enabled = FALSE;
+        markers[t].enabled = M_DISABLED;
       previous_marker = -1;
       active_marker = -1;      
   } else if (item == 5) { /* marker delta */
@@ -1088,7 +1091,7 @@ ensure_selection(void)
 {
   const menuitem_t *menu = menu_stack[menu_current_level];
   int i;
-  for (i = 0; MT_MASK(menu[i].type) != MT_NONE; i++)
+  for (i = 0; MT_MASK(menu[i].type) != MT_NONE && MT_MASK(menu[i].type) != MT_TITLE  ; i++)
     ;
   if (selection >= i)
     selection = i-1;
@@ -1145,10 +1148,11 @@ menu_move_top(void)
 static void
 menu_invoke(int item)
 {
+  int status;
   const menuitem_t *menu = menu_stack[menu_current_level];
   menu = &menu[item];
 
-  switch (menu->type & 0x0f) {
+  switch (MT_MASK(menu->type)) {
   case MT_NONE:
   case MT_BLANK:
   case MT_CLOSE:
@@ -1164,12 +1168,30 @@ menu_invoke(int item)
     if (cb == NULL)
       return;
     (*cb)(item, menu->data);
+    if (!(menu->type & MT_FORM))
+      draw_cal_status();
     break;
   }
 
   case MT_SUBMENU:
     menu_push_submenu((const menuitem_t*)menu->reference);
     break;
+
+  case MT_KEYPAD:
+    status = btn_wait_release();
+    if (status & EVT_BUTTON_DOWN_LONG) {
+      ui_mode_numeric(menu->data);
+      //    ui_process_numeric();
+    } else {
+      if (menu->type & MT_FORM) {
+        area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;
+        redraw_frame();         // Remove form numbers
+      }
+      ui_mode_keypad(menu->data);
+      ui_process_keypad();
+    }
+    draw_cal_status();
+    break;
   }
 }
 
@@ -1306,14 +1328,22 @@ draw_keypad(void)
     i++;
   }
 }
+static int
+menu_is_multiline(const char *label, const char **l1, const char **l2);
 
 static void
 draw_numeric_area_frame(void)
 {
+  const char *l1;
+  const char *l2;
   ili9341_fill(0, 240-NUM_INPUT_HEIGHT, 320, NUM_INPUT_HEIGHT, config.menu_normal_color);
   ili9341_set_foreground(DEFAULT_MENU_TEXT_COLOR);
   ili9341_set_background(config.menu_normal_color);
-  ili9341_drawstring(keypad_mode_label[keypad_mode], 10, 240-(FONT_GET_HEIGHT+NUM_INPUT_HEIGHT)/2);
+  if (menu_is_multiline(keypad_mode_label[keypad_mode], &l1, &l2)) {
+    ili9341_drawstring_7x13(l1, 10, 240-NUM_INPUT_HEIGHT+1);
+    ili9341_drawstring_7x13(l2, 10, 240-NUM_INPUT_HEIGHT/2 + 1);
+  } else
+    ili9341_drawstring_7x13(keypad_mode_label[keypad_mode], 10, 240-(FONT_GET_HEIGHT+NUM_INPUT_HEIGHT)/2);
   //ili9341_drawfont(KP_KEYPAD, 300, 216);
 }
 
@@ -1482,7 +1512,7 @@ draw_menu_buttons(const menuitem_t *menu)
     if (menu[i].type & MT_FORM) {
       active_button_start = 320 - MENU_FORM_WIDTH;
       active_button_width = MENU_FORM_WIDTH - 30;       // Shorten at the right
-      if (MT_MASK(menu[i].type) == MT_CALLBACK) {       // Only callback can have value
+      if (MT_MASK(menu[i].type) == MT_KEYPAD) {         // Only keypad retrieves value
         keypad_mode = menu[i].data;
         fetch_numeric_target();
       }
@@ -1737,7 +1767,7 @@ ui_mode_numeric(int _keypad_mode)
 static void
 ui_mode_keypad(int _keypad_mode)
 {
-  if (ui_mode == UI_KEYPAD)
+  if (ui_mode == UI_KEYPAD && keypad_mode == _keypad_mode )
     return;
 
   // keypads array
@@ -2307,6 +2337,7 @@ void ui_process_touch(void)
       // switch menu mode after release
       touch_wait_release();
       selection = -1; // hide keyboard mode selection
+      ensure_selection();
       ui_mode_menu();
       break;
     case UI_MENU:
@@ -2329,6 +2360,7 @@ ui_process(void)
   int button_state = READ_PORT() & BUTTON_MASK;
   if (ui_mode == UI_NORMAL && current_menu_is_form()) {     //   Force into menu mode
     selection = -1; // hide keyboard mode selection
+    ensure_selection();
     ui_mode_menu();
   }
   if (operation_requested&OP_LEVER || previous_button_state != button_state) {
diff --git a/ui_sa.c b/ui_sa.c
index d997f30..58875ca 100644
--- a/ui_sa.c
+++ b/ui_sa.c
@@ -167,7 +167,7 @@ static const char * const keypad_mode_label[] = {
 #endif
 #ifdef __SA__
 static const char * const keypad_mode_label[] = {
-  "error", "START", "STOP", "CENTER", "SPAN", "CW FREQ", "REFPOS", "SCALE", "ATTENUATION", "ACTUALPOWER", "IF", "SAMPLE TIME", "DRIVE", "LEVEL", "LEVEL"
+  "error", "START", "STOP", "CENTER", "SPAN", "CW FREQ", "REFPOS", "SCALE", "\2ATTENUATE\0 0-31dB", "ACTUALPOWER", "IF", "SAMPLE TIME", "DRIVE", "LEVEL", "LEVEL"
 };
 #endif
 
@@ -216,9 +216,9 @@ void menu_autosettings_cb(int item, uint8_t data)
 
   active_marker = 0;
   for (int i = 1; i<MARKER_COUNT; i++ ) {
-    markers[i].enabled = false;
+    markers[i].enabled = M_DISABLED;
   }
-  markers[0].enabled = true;
+  markers[0].enabled = M_TRACKING_ENABLED;
   markers[0].mtype = M_REFERENCE;
 
   //  set_refer_output(1);
@@ -228,7 +228,7 @@ void menu_autosettings_cb(int item, uint8_t data)
   dirty = true;
   //  menu_move_back();   // stay in input menu
   ui_mode_normal();
-  draw_cal_status();
+//  draw_cal_status();
 }
 
 static void menu_calibrate_cb(int item, uint8_t data)
@@ -285,16 +285,18 @@ static void menu_dfu_cb(int item, uint8_t data)
   }
 }
 
-int menu_modulation_value[]={0, MO_NONE,MO_AM, MO_NFM, MO_WFM};
-char *menu_modulation_text[]={"NONE","AM","NARROW FM","WIDE FM"};
+
+const int menu_modulation_value[]={MO_NONE,MO_AM, MO_NFM, MO_WFM};
+const char *menu_modulation_text[]={"NONE","AM","NARROW FM","WIDE FM"};
+
 static void menu_modulation_cb(int item, uint8_t data)
 {
-  (void)data;
+  (void)item;
 //Serial.println(item);
-  SetModulation(menu_modulation_value[item]);
+  SetModulation(menu_modulation_value[data]);
   menu_move_back();
 //  ui_mode_normal();   // Stay in menu mode
-  draw_cal_status();
+//  draw_cal_status();
 }
 
 
@@ -302,29 +304,34 @@ const int menu_reffer_value[]={-1,0,1,2,3,4,5,6};
 const char *menu_reffer_text[]={"OFF","30MHz","15MHz","10MHz","4MHz","3MHz","2MHz","1MHz"};
 static void menu_reffer_cb(int item, uint8_t data)
 {
-  (void)data;
+  (void)item;
 //Serial.println(item);
-  set_refer_output(menu_reffer_value[item]);
+  set_refer_output(menu_reffer_value[data]);
   menu_move_back();
 //  ui_mode_normal();   // Stay in menu mode
-  draw_cal_status();
+//  draw_cal_status();
 }
 
-static void menu_reffer_cb2(int item, uint8_t data)
+const int menu_drive_value[]={5,10,15,20};
+const char *menu_drive_text[]={"5dBm","10dBm","15dBm","20dBm"};
+static void menu_drive_cb(int item, uint8_t data)
 {
-  (void)data;
+  (void)item;
 //Serial.println(item);
-  set_refer_output(menu_reffer_value[item+5]);
+  SetDrive(data);
   menu_move_back();
-  //  ui_mode_normal();   // Stay in menu mode
-  draw_cal_status();
+//  ui_mode_normal();
+//  draw_cal_status();
 }
 
+
+
+#if 0
+
 static void menu_spur_cb(int item, uint8_t data)
 {
   (void)data;
   (void)item;
-#if 0
   if (GetSpur())
     SetSpur(0);
   else
@@ -332,13 +339,49 @@ static void menu_spur_cb(int item, uint8_t data)
 //  menu_move_back();
   ui_mode_normal();
   draw_cal_status();
+}
 #endif
+
+static void menu_measure_cb(int item, uint8_t data)
+{
+  (void)item;
+  switch(data) {
+    case 0:                                     // IMD
+      for (int i = 0; i< MARKERS_MAX; i++) {
+        markers[i].enabled = M_TRACKING_ENABLED;
+        markers[i].mtype = M_DELTA;
+      }
+      markers[0].mtype = M_REFERENCE;
+      ui_mode_keypad(KM_CENTER);
+      ui_process_keypad();
+      set_sweep_frequency(ST_START, 0);
+      set_sweep_frequency(ST_STOP, uistat.value*5);
+      break;
+    case 1:
+      for (int i = 0; i< MARKERS_MAX; i++) {
+        markers[i].enabled = M_TRACKING_ENABLED;
+        markers[i].mtype = M_DELTA;
+      }
+      markers[0].mtype = M_REFERENCE;
+      ui_mode_keypad(KM_CENTER);
+      ui_process_keypad();
+      ui_mode_keypad(KM_SPAN);
+      ui_process_keypad();
+      break;
+    case 2:
+      break;
+    case 3:
+      break;
+  }
+  menu_move_back();
+  ui_mode_normal();
+//  draw_cal_status();
 }
 
 static void menu_storage_cb(int item, uint8_t data)
 {
-  (void)data;
-  switch(item) {
+  (void)item;
+  switch(data) {
     case 0:
       SetStorage();
       break;
@@ -354,7 +397,7 @@ static void menu_storage_cb(int item, uint8_t data)
   }
   menu_move_back();
   ui_mode_normal();
-  draw_cal_status();
+//  draw_cal_status();
 }
 
 static void menu_average_cb(int item, uint8_t data)
@@ -368,16 +411,22 @@ static void menu_average_cb(int item, uint8_t data)
 
 static void menu_marker_type_cb(int item, uint8_t data)
 {
-  (void)data;
+  (void)item;
   if (markers[active_marker].enabled)
   {
-    if (item == M_REFERENCE) {
+    if (data == M_REFERENCE) {
       for (int i = 0; i<MARKER_COUNT; i++ ) {
         if (markers[i].mtype ==M_REFERENCE)
           markers[i].mtype = M_NORMAL;
       }
     }
-    markers[active_marker].mtype = item;
+    if (data == M_TRACKING) {
+      if (markers[active_marker].enabled == M_ENABLED)
+        markers[active_marker].enabled = M_TRACKING_ENABLED;
+      else
+        markers[active_marker].enabled = M_ENABLED;
+    } else
+      markers[active_marker].mtype = data;
   }
   markmap_all_markers();
 //  redraw_marker(active_marker, TRUE);
@@ -390,22 +439,22 @@ const int rbwsel[]={0,3,10,30,100,300};
 
 static void menu_rbw_cb(int item, uint8_t data)
 {
-  (void)data;
-  SetRBW(rbwsel[item]); 
+  (void)item;
+  SetRBW(rbwsel[data]);
   menu_move_back();
   ui_mode_normal();
-  draw_cal_status();
+//  draw_cal_status();
 }
 
 int menu_dBper_value[]={1,2,5,10,20};
 
 static void menu_dBper_cb(int item, uint8_t data)
 {
-  (void)data;
-  SetScale(menu_dBper_value[item]);
+  (void)item;
+  SetScale(menu_dBper_value[data]);
   menu_move_back();
   ui_mode_normal();
-  draw_cal_status();
+//  draw_cal_status();
 }
 
 #if 0
@@ -433,87 +482,6 @@ static void choose_active_marker(void)
   active_marker = -1;
 }
 
-static void menu_scale_cb(int item, uint8_t data)
-{
-  (void)item;
-  int status;
-  status = btn_wait_release();
-  if (status & EVT_BUTTON_DOWN_LONG) {
-    ui_mode_numeric(data);
-//    ui_process_numeric();
-  } else {
-    ui_mode_keypad(data);
-    ui_process_keypad();
-  }
-  draw_cal_status();
-}
-
-static void menu_lowoutputmode_cb(int item, uint8_t data)
-{
-  int status;
-  int km = data;
-  (void) item;
-//  if (km == KM_SCALE && trace[uistat.current_trace].type == TRC_DELAY) {
-//    km = KM_SCALEDELAY;
-//  }
-
-  status = btn_wait_release();
-
-  if (item == 3) {
-    menu_push_submenu(menu_modulation);
-  } else
-  {
-    if (status & EVT_BUTTON_DOWN_LONG) {
-      ui_mode_numeric(km);
-      //    ui_process_numeric();
-    } else {
-      area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;
-      redraw_frame();         // Remove form numbers
-      ui_mode_keypad(km);
-      ui_process_keypad();
-    }
-  }
-  draw_cal_status();
-}
-#if 0
-static void menu_highoutputmode_cb(int item, uint8_t data)
-{
-  int status;
-  int km = data;
-  (void) item;
-
-//  if (km == KM_SCALE && trace[uistat.current_trace].type == TRC_DELAY) {
-//    km = KM_SCALEDELAY;
-//  }
-  status = btn_wait_release();
-  if (status & EVT_BUTTON_DOWN_LONG) {
-    ui_mode_numeric(km);
-//    ui_process_numeric();
-  } else {
-    area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;
-    redraw_frame();         // Remove form numbers
-    ui_mode_keypad(km);
-    ui_process_keypad();
-  }
-  draw_cal_status();
-}
-#endif
-
-static void menu_settings_cb(int item, uint8_t data)
-{
-  (void)item;
-  int status;
-  status = btn_wait_release();
-  if (status & EVT_BUTTON_DOWN_LONG) {
-    ui_mode_numeric(data);
-    //    ui_process_numeric();
-  } else {
-    ui_mode_keypad(data);
-    ui_process_keypad();
-  }
-  draw_cal_status();
-}
-
 static void menu_settings2_cb(int item, uint8_t data)
 {
   (void)data;
@@ -528,97 +496,97 @@ static void menu_settings2_cb(int item, uint8_t data)
     toggle_tracking();
     break;
   }
-  draw_cal_status();
   draw_menu();
+//  draw_cal_status();
 }
 
-static void menu_stimulus_cb(int item, uint8_t data)
+static void menu_pause_cb(int item, uint8_t data)
 {
   (void) data;
-  int status;
-  int km = item+KM_START;
-  switch (km) {
-  case KM_START: /* START */
-  case KM_STOP: /* STOP */
-  case KM_CENTER: /* CENTER */
-  case KM_SPAN: /* SPAN */
-  case KM_CW: /* CW */
-    status = btn_wait_release();
-    if (status & EVT_BUTTON_DOWN_LONG) {
-      ui_mode_numeric(km);
-//      ui_process_numeric();
-    } else {
-      ui_mode_keypad(km);
-      ui_process_keypad();
-    }
-    break;
-  case KM_CW+1: /* PAUSE */
-    toggle_sweep();
-    menu_move_back();
-    ui_mode_normal();
-    draw_menu();
-    break;
-  }
-  draw_cal_status();
+  (void) item;
+  toggle_sweep();
+  menu_move_back();
+  ui_mode_normal();
+  draw_menu();
+//  draw_cal_status();
 }
 
 // ===[MENU DEFINITION]=========================================================
 
+static const menuitem_t menu_drive[] = {
+  { MT_CALLBACK, 0, "  5dBm",   menu_drive_cb},
+  { MT_CALLBACK, 1, " 10dBm",   menu_drive_cb},
+  { MT_CALLBACK, 2, " 15dBm",   menu_drive_cb},
+  { MT_CALLBACK, 3, " 20dBm",   menu_drive_cb},
+  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
+
+static const menuitem_t menu_drive_wide[] = {
+  { MT_FORM | MT_CALLBACK, 0, "  5dBm",   menu_drive_cb},
+  { MT_FORM | MT_CALLBACK, 1, " 10dBm",   menu_drive_cb},
+  { MT_FORM | MT_CALLBACK, 2, " 15dBm",   menu_drive_cb},
+  { MT_FORM | MT_CALLBACK, 3, " 20dBm",   menu_drive_cb},
+  { MT_FORM | MT_CANCEL,   0, S_LARROW" BACK", NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
+
 const menuitem_t  menu_modulation[] = {
   { MT_FORM | MT_TITLE,    0,  "MODULATION",NULL},
   { MT_FORM | MT_CALLBACK, 0,  "NONE",      menu_modulation_cb},
-  { MT_FORM | MT_CALLBACK, 0,  "AM",        menu_modulation_cb},
-  { MT_FORM | MT_CALLBACK, 0,  "NARROW FM", menu_modulation_cb},
-  { MT_FORM | MT_CALLBACK, 0,  "WIDE FM",   menu_modulation_cb},
+  { MT_FORM | MT_CALLBACK, 1,  "AM",        menu_modulation_cb},
+  { MT_FORM | MT_CALLBACK, 2,  "NARROW FM", menu_modulation_cb},
+  { MT_FORM | MT_CALLBACK, 3,  "WIDE FM",   menu_modulation_cb},
   { MT_FORM | MT_CANCEL,   0,             S_LARROW" BACK",NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
 
 const menuitem_t  menu_lowoutputmode[] = {
-  { MT_FORM | MT_TITLE,    0,             "LOW OUTPUT",  NULL},
-  { MT_FORM | MT_CALLBACK, KM_CENTER,     "FREQ: %s",    menu_lowoutputmode_cb},
-  { MT_FORM | MT_CALLBACK, KM_LOWOUTLEVEL, "LEVEL: %s",   menu_lowoutputmode_cb},
-  { MT_FORM | MT_SUBMENU,  0,              "MODULATION: %s",   menu_modulation},
-  { MT_FORM | MT_CANCEL,   0,             S_LARROW" BACK",NULL },
+  { MT_FORM | MT_TITLE,    0,               "LOW OUTPUT",       NULL},
+  { MT_FORM | MT_KEYPAD,   KM_CENTER,       "FREQ: %s",         NULL},
+  { MT_FORM | MT_KEYPAD,   KM_LOWOUTLEVEL,  "LEVEL: %s",        NULL},
+  { MT_FORM | MT_SUBMENU,  0,               "MODULATION: %s",   menu_modulation},
+  { MT_FORM | MT_CANCEL,   0,           S_LARROW" BACK",    NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
 
 const menuitem_t  menu_highoutputmode[] = {
-  { MT_FORM | MT_TITLE,    0,             "HIGH OUTPUT", NULL},
-  { MT_FORM | MT_CALLBACK, KM_CENTER,     "FREQ: %s",    menu_lowoutputmode_cb},    // same menu as low mode
-  { MT_FORM | MT_CALLBACK, KM_HIGHOUTLEVEL,      "LEVEL: %s",   menu_lowoutputmode_cb},
-  { MT_FORM | MT_SUBMENU,  0,              "MODULATION: %s",   menu_modulation},
-  { MT_FORM | MT_CANCEL,   0,             S_LARROW" BACK",NULL },
+  { MT_FORM | MT_TITLE,     0,          "HIGH OUTPUT",      NULL},
+  { MT_FORM | MT_KEYPAD,    KM_CENTER,  "FREQ: %s",         NULL},
+  { MT_FORM | MT_SUBMENU,   0,          "LEVEL: %s",        menu_drive_wide},
+  { MT_FORM | MT_SUBMENU,   0,          "MODULATION: %s",   menu_modulation},
+  { MT_FORM | MT_CANCEL,    0,          S_LARROW" BACK",NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
 
 static const menuitem_t  menu_average[] = {
-  { MT_CALLBACK, 0, " OFF",   menu_average_cb},
-  { MT_CALLBACK, 0, "\2 MIN\0 HOLD",   menu_average_cb},
-  { MT_CALLBACK, 0, "\2 MAX\0 HOLD",   menu_average_cb},
-  { MT_CALLBACK, 0, "\2 MAX\0 DECAY",   menu_average_cb},
-  { MT_CALLBACK, 0, " 4 ",   menu_average_cb},
-  { MT_CALLBACK, 0, " 16 ",   menu_average_cb},
+  { MT_CALLBACK, 0, " OFF",             menu_average_cb},
+  { MT_CALLBACK, 1, "\2 MIN\0 HOLD",    menu_average_cb},
+  { MT_CALLBACK, 2, "\2 MAX\0 HOLD",    menu_average_cb},
+  { MT_CALLBACK, 3, "\2 MAX\0 DECAY",   menu_average_cb},
+  { MT_CALLBACK, 4, " 4 ",              menu_average_cb},
+  { MT_CALLBACK, 5, " 16 ",             menu_average_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
-
+#if 0
 static const menuitem_t menu_storage[] = {
   { MT_CALLBACK, 0, "STORE",    menu_storage_cb},
-  { MT_CALLBACK, 0, "CLEAR",    menu_storage_cb},
-  { MT_CALLBACK, 0, "SUBTRACT", menu_storage_cb},
-  { MT_CALLBACK, 0, "WATERFALL",menu_storage_cb},
+  { MT_CALLBACK, 1, "CLEAR",    menu_storage_cb},
+  { MT_CALLBACK, 2, "SUBTRACT", menu_storage_cb},
+  { MT_CALLBACK, 3, "WATERFALL",menu_storage_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,      0, NULL, NULL } // sentinel
 };
+#endif
 
 static const menuitem_t menu_rbw[] = {
   { MT_CALLBACK, 0, "  AUTO",   menu_rbw_cb},
-  { MT_CALLBACK, 0, "  3kHz",   menu_rbw_cb},
-  { MT_CALLBACK, 0, " 10kHz",   menu_rbw_cb},
-  { MT_CALLBACK, 0, " 30kHz",   menu_rbw_cb},
-  { MT_CALLBACK, 0, "100kHz",   menu_rbw_cb},
-  { MT_CALLBACK, 0, "300kHz",   menu_rbw_cb},
+  { MT_CALLBACK, 1, "  3kHz",   menu_rbw_cb},
+  { MT_CALLBACK, 2, " 10kHz",   menu_rbw_cb},
+  { MT_CALLBACK, 3, " 30kHz",   menu_rbw_cb},
+  { MT_CALLBACK, 4, "100kHz",   menu_rbw_cb},
+  { MT_CALLBACK, 5, "300kHz",   menu_rbw_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,      0, NULL, NULL } // sentinel
 };
@@ -626,93 +594,123 @@ static const menuitem_t menu_rbw[] = {
 
 static const menuitem_t menu_dBper[] = {
   { MT_CALLBACK, 0, "  1dB/",   menu_dBper_cb},
-  { MT_CALLBACK, 0, "  2dB/",   menu_dBper_cb},
-  { MT_CALLBACK, 0, "  5dB/",   menu_dBper_cb},
-  { MT_CALLBACK, 0, " 10dB/",   menu_dBper_cb},
-  { MT_CALLBACK, 0, " 20dB/",   menu_dBper_cb},
+  { MT_CALLBACK, 1, "  2dB/",   menu_dBper_cb},
+  { MT_CALLBACK, 2, "  5dB/",   menu_dBper_cb},
+  { MT_CALLBACK, 3, " 10dB/",   menu_dBper_cb},
+  { MT_CALLBACK, 4, " 20dB/",   menu_dBper_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
 static const menuitem_t menu_reffer2[] = {
-  { MT_FORM | MT_CALLBACK, 0, "3MHz" ,   menu_reffer_cb2},
-  { MT_FORM | MT_CALLBACK, 0, "2MHz" ,   menu_reffer_cb2},
-  { MT_FORM | MT_CALLBACK, 0, "1MHz" ,   menu_reffer_cb2},
+  { MT_FORM | MT_CALLBACK, 5, "3MHz" ,   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 6, "2MHz" ,   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 7, "1MHz" ,   menu_reffer_cb},
   { MT_FORM | MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
 static const menuitem_t menu_reffer[] = {
   { MT_FORM | MT_CALLBACK, 0, "OFF"  ,   menu_reffer_cb},
-  { MT_FORM | MT_CALLBACK, 0, "30MHz",   menu_reffer_cb},
-  { MT_FORM | MT_CALLBACK, 0, "15MHz",   menu_reffer_cb},
-  { MT_FORM | MT_CALLBACK, 0, "10MHz",   menu_reffer_cb},
-  { MT_FORM | MT_CALLBACK, 0, "4MHz" ,   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 1, "30MHz",   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 2, "15MHz",   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 3, "10MHz",   menu_reffer_cb},
+  { MT_FORM | MT_CALLBACK, 4, "4MHz" ,   menu_reffer_cb},
   { MT_FORM | MT_SUBMENU,  0, S_RARROW" MORE", menu_reffer2},
   { MT_FORM | MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
+static const menuitem_t menu_acquire[] = {
+  { MT_CALLBACK, 0, "AUTO",     menu_autosettings_cb},
+  { MT_KEYPAD, KM_ATTENUATION,  "ATTEN",            NULL},
+  { MT_SUBMENU,0,               "RBW",              menu_rbw},
+  { MT_SUBMENU,0,               "AVERAGE",          menu_average},
+  { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
+};
+
+static const menuitem_t menu_acquirehigh[] = {
+  { MT_CALLBACK, 0, "AUTO",     menu_autosettings_cb},
+  { MT_SUBMENU,0,               "RBW",              menu_rbw},
+  { MT_SUBMENU,0,               "AVERAGE",          menu_average},
+  { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
+};
+
+
+static const menuitem_t menu_display[] = {
+  { MT_KEYPAD, KM_REFPOS,       "\2REF\0LEVEL",     NULL},
+  { MT_SUBMENU,0,               "\2SCALE/\0DIV",    menu_dBper},
+  { MT_CALLBACK, 0, "STORE",    menu_storage_cb},
+  { MT_CALLBACK, 1, "CLEAR",    menu_storage_cb},
+  { MT_CALLBACK, 2, "SUBTRACT", menu_storage_cb},
+  { MT_CALLBACK, 3, "WATERFALL",menu_storage_cb},
+  { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
+};
+
+#if 0
 static const menuitem_t menu_scale[] = {
-  { MT_CALLBACK, KM_REFPOS,         "\2REF\0LEVEL",  menu_scale_cb},
-  { MT_SUBMENU,  0,                 "\2SCALE/\0DIV",     menu_dBper},
-  { MT_CALLBACK, KM_ATTENUATION,    "ATTEN",         menu_scale_cb},
-  { MT_SUBMENU,  0,                 "AVERAGE",       menu_average},
-  { MT_CALLBACK, 0,                 "\2SPUR\0REDUCT.",menu_spur_cb},
-  { MT_SUBMENU,  0,                 "RBW",           menu_rbw},
-  { MT_CANCEL,   0,                 S_LARROW" BACK", NULL },
-  { MT_NONE,     0, NULL, NULL } // sentinel
+  { MT_KEYPAD, KM_REFPOS,       "\2REF\0LEVEL",     NULL},
+  { MT_SUBMENU,0,               "\2SCALE/\0DIV",    menu_dBper},
+  { MT_KEYPAD, KM_ATTENUATION,  "ATTEN",            NULL},
+  { MT_SUBMENU,0,               "AVERAGE",          menu_average},
+  { MT_KEYPAD, 0,               "\2SPUR\0REDUCT.",  menu_spur_cb},
+  { MT_SUBMENU,0,               "RBW",              menu_rbw},
+  { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
 };
 
 static const menuitem_t menu_scalehigh[] = {
-  { MT_CALLBACK, KM_REFPOS, "\2REF\0LEVEL",  menu_scale_cb},
-  { MT_SUBMENU,  0,         "\2SCALE/\0DIV", menu_dBper},
-  { MT_SUBMENU,  0,         "AVERAGE",       menu_average},
-  { MT_SUBMENU,  0,         "RBW",           menu_rbw},
-  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
-  { MT_NONE,     0, NULL, NULL } // sentinel
+  { MT_KEYPAD, KM_REFPOS,   "\2REF\0LEVEL",  NULL},
+  { MT_SUBMENU,0,           "\2SCALE/\0DIV", menu_dBper},
+  { MT_SUBMENU,0,           "AVERAGE",       menu_average},
+  { MT_SUBMENU,0,           "RBW",           menu_rbw},
+  { MT_CANCEL, 0, S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
 };
 
+#endif
 
-
-static const menuitem_t menu_stimulus[] = {
-  { MT_CALLBACK, 0, "START",            menu_stimulus_cb},
-  { MT_CALLBACK, 0, "STOP",             menu_stimulus_cb},
-  { MT_CALLBACK, 0, "CENTER",           menu_stimulus_cb},
-  { MT_CALLBACK, 0, "SPAN",             menu_stimulus_cb},
-  { MT_CALLBACK, 0, "CW FREQ",          menu_stimulus_cb},
-//  { MT_SUBMENU,  0, "RBW",              menu_rbw},
-  { MT_CALLBACK, 0, "\2PAUSE\0SWEEP",   menu_stimulus_cb},
-  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
-  { MT_NONE,     0, NULL, NULL } // sentinel
+static const menuitem_t menu_stimulus[8] = {
+  { MT_KEYPAD,  KM_START,   "START",            NULL},
+  { MT_KEYPAD,  KM_STOP,    "STOP",             NULL},
+  { MT_KEYPAD,  KM_CENTER,  "CENTER",           NULL},
+  { MT_KEYPAD,  KM_SPAN,    "SPAN",             NULL},
+  { MT_KEYPAD,  KM_CW,      "CW FREQ",          NULL},
+  { MT_CALLBACK,0,          "\2PAUSE\0SWEEP",   menu_pause_cb},
+  { MT_CANCEL,  0,          S_LARROW" BACK", NULL },
+  { MT_NONE,    0, NULL, NULL } // sentinel
 };
 
 
 static const menuitem_t menu_marker_type[] = {
-  { MT_CALLBACK, 0, "REFERENCE",    menu_marker_type_cb},
-  { MT_CALLBACK, 0, "NORMAL",       menu_marker_type_cb},
-  { MT_CALLBACK, 0, "DELTA",        menu_marker_type_cb},
+  { MT_CALLBACK, M_REFERENCE,   "REFERENCE",    menu_marker_type_cb},
+  { MT_CALLBACK, M_NORMAL,      "NORMAL",       menu_marker_type_cb},
+  { MT_CALLBACK, M_DELTA,       "DELTA",        menu_marker_type_cb},
+  { MT_CALLBACK, M_TRACKING,    "TRACKING",     menu_marker_type_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
 const menuitem_t menu_marker_search[] = {
   //{ MT_CALLBACK, "OFF", menu_marker_search_cb },
-  { MT_CALLBACK, 0, "MAXIMUM", menu_marker_search_cb },
-  { MT_CALLBACK, 0, "MINIMUM", menu_marker_search_cb },
-  { MT_CALLBACK, 0, "\2SEARCH\0" S_LARROW" LEFT", menu_marker_search_cb },
-  { MT_CALLBACK, 0, "\2SEARCH\0" S_RARROW" RIGHT", menu_marker_search_cb },
-  { MT_CALLBACK, 0, "TRACKING", menu_marker_search_cb },
+  { MT_CALLBACK, 0, "MAXIMUM",                      menu_marker_search_cb },
+  { MT_CALLBACK, 1, "MINIMUM",                      menu_marker_search_cb },
+  { MT_CALLBACK, 2, "\2SEARCH\0" S_LARROW" LEFT",   menu_marker_search_cb },
+  { MT_CALLBACK, 3, "\2SEARCH\0" S_RARROW" RIGHT",  menu_marker_search_cb },
+  { MT_CALLBACK, 4, "TRACKING",                     menu_marker_search_cb },
   { MT_CANCEL, 0, S_LARROW" BACK", NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
 
 static const menuitem_t menu_marker_sel[] = {
-  { MT_CALLBACK, 0, "MARKER 1",     menu_marker_sel_cb},
-  { MT_CALLBACK, 0, "MARKER 2",     menu_marker_sel_cb},
-  { MT_CALLBACK, 0, "MARKER 3",     menu_marker_sel_cb},
-  { MT_CALLBACK, 0, "MARKER 4",     menu_marker_sel_cb},
-  { MT_CALLBACK, 0, "ALL OFF",      menu_marker_sel_cb},
+  { MT_CALLBACK, 0, "MARKER 1",             menu_marker_sel_cb},
+  { MT_CALLBACK, 0, "MARKER 2",             menu_marker_sel_cb},
+  { MT_CALLBACK, 0, "MARKER 3",             menu_marker_sel_cb},
+  { MT_CALLBACK, 0, "MARKER 4",             menu_marker_sel_cb},
+  { MT_CALLBACK, 0, "ALL OFF",              menu_marker_sel_cb},
   { MT_SUBMENU,  0, "\2SEARCH\0MARKER",     menu_marker_search},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
@@ -730,8 +728,8 @@ static const menuitem_t menu_marker[] = {
 };
 
 static const menuitem_t menu_dfu[] = {
-  { MT_FORM | MT_CALLBACK, 0, "ENTER DFU", menu_dfu_cb},
-  { MT_FORM | MT_CANCEL,   0, S_LARROW" BACK", NULL },
+  { MT_FORM | MT_CALLBACK, 0, "ENTER DFU",      menu_dfu_cb},
+  { MT_FORM | MT_CANCEL,   0, S_LARROW" BACK",  NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
@@ -746,15 +744,21 @@ static const menuitem_t menu_settings2[] =
 
 static const menuitem_t menu_settings[] =
 {
-  { MT_CALLBACK, KM_ACTUALPOWER,    "\2ACTUAL\0POWER",  menu_settings_cb},
-  { MT_CALLBACK, KM_IF,             "\2IF\0FREQ",       menu_settings_cb},
-  { MT_CALLBACK, KM_SAMPLETIME,     "\2SAMPLE\0TIME",   menu_settings_cb},
-  { MT_CALLBACK, KM_DRIVE,          "\2LO\0DRIVE",      menu_settings_cb},
+  { MT_KEYPAD, KM_ACTUALPOWER,    "\2ACTUAL\0POWER",  NULL},
+  { MT_KEYPAD, KM_IF,             "\2IF\0FREQ",       NULL},
+  { MT_KEYPAD, KM_SAMPLETIME,     "\2SAMPLE\0TIME",   NULL},
+  { MT_SUBMENU,0,                 "\2LO\0DRIVE",      menu_drive},
   { MT_SUBMENU,  0,                 S_RARROW" MORE",    menu_settings2},
   { MT_CANCEL,   0,                 S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
+static const menuitem_t menu_measure[] = {
+  { MT_CALLBACK, 0, "IMD",      menu_measure_cb},
+  { MT_CALLBACK, 1, "IIP3",     menu_measure_cb},
+  { MT_CANCEL, 0,               S_LARROW" BACK", NULL },
+  { MT_NONE,   0, NULL, NULL } // sentinel
+};
 
 static const menuitem_t menu_settingshigh2[] =
 {
@@ -766,8 +770,8 @@ static const menuitem_t menu_settingshigh2[] =
 
 static const menuitem_t menu_settingshigh[] =
 {
-  { MT_CALLBACK, KM_ACTUALPOWER,    "\2ACTUAL\0POWER",  menu_settings_cb},
-  { MT_CALLBACK, KM_SAMPLETIME,     "\2SAMPLE\0TIME",   menu_settings_cb},
+  { MT_KEYPAD, KM_ACTUALPOWER,    "\2ACTUAL\0POWER",  NULL},
+  { MT_KEYPAD, KM_SAMPLETIME,     "\2SAMPLE\0TIME",   NULL},
   { MT_SUBMENU,  0,                 S_RARROW" MORE",    menu_settingshigh2},
   { MT_CANCEL,   0,                 S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
@@ -806,7 +810,32 @@ static const menuitem_t menu_mode[] = {
 //  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
+#if 1
+const menuitem_t menu_top[] = {
+  { MT_SUBMENU,  0, "ACQUIRE",      menu_acquire},
+  { MT_SUBMENU,  0, "SCAN",         menu_stimulus},
+  { MT_SUBMENU,  0, "DISPLAY",      menu_display},
+  { MT_SUBMENU,  0, "MARKER",       menu_marker},
+  { MT_SUBMENU,  0, "MEASURE",      menu_measure},
+  { MT_SUBMENU,  0, "SETTINGS",     menu_settings},
+  { MT_CANCEL,   0, S_LARROW" MODE",NULL},
+  { MT_NONE,     0, NULL, NULL } // sentinel,
+ // MENUITEM_CLOSE,
+};
 
+const menuitem_t menu_tophigh[] =
+{
+ { MT_SUBMENU,  0, "ACQUIRE",      menu_acquirehigh},
+ { MT_SUBMENU,  0, "SCAN",         menu_stimulus},
+ { MT_SUBMENU,  0, "DISPLAY",      menu_display},
+ { MT_SUBMENU,  0, "MARKER",       menu_marker},
+ { MT_SUBMENU,  0, "MEASURE",      menu_measure},
+ { MT_SUBMENU,  0, "SETTINGS",     menu_settings},
+ { MT_CANCEL,   0, S_LARROW" MODE",NULL},
+ { MT_NONE,     0, NULL, NULL } // sentinel,
+ // MENUITEM_CLOSE,
+};
+#else
 const menuitem_t menu_top[] = {
   { MT_CALLBACK, 0, "AUTO",         menu_autosettings_cb},
   { MT_SUBMENU,  0, "SCAN",         menu_stimulus},
@@ -830,7 +859,7 @@ const menuitem_t menu_tophigh[] = {
   { MT_NONE,     0, NULL, NULL } // sentinel,
  // MENUITEM_CLOSE,
 };
-
+#endif
 // ===[MENU DEFINITION END]======================================================
 
 #undef BOARD_NAME
@@ -853,122 +882,87 @@ int menu_is_form(const menuitem_t *menu)
 static void menu_item_modify_attribute(
     const menuitem_t *menu, int item, uint16_t *fg, uint16_t *bg)
 {
+  int mark = false;
   if (menu == menu_mode) {
-    if (item == GetMode()+1) {
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+    if (item == GetMode()+1)  {
+      mark = true;
     } else if (item == 5) {
       plot_printf(uistat.text, sizeof uistat.text, menu_reffer_text[get_refer_output()+1]);
     }
+  } else if (menu == menu_highoutputmode && item == 2) {
+      plot_printf(uistat.text, sizeof uistat.text, menu_drive_text[setting_drive]);
   } else if (menu == menu_lowoutputmode || menu == menu_highoutputmode) {
     if (item == 3) {
       plot_printf(uistat.text, sizeof uistat.text, menu_modulation_text[setting_modulation]);
     }
   } else if (menu == menu_reffer) {
     if (item < 5 && item == get_refer_output() + 1){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
-    }
+      mark = true;
+   }
   } else if (menu == menu_reffer2) {
     if (item == get_refer_output() - 4){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
   } else if (menu == menu_stimulus) {
     if (item == 5 /* PAUSE */ && !(sweep_mode&SWEEP_ENABLE)) {
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
-    }
-  } else if (menu == menu_scale) {
-#if 0
-    if (item == 4 /* Spur reduction */ && GetSpur()) {
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
-#endif
   } else if (menu == menu_average) {
     if (item == GetAverage()){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
   } else if (menu == menu_dBper) {
     if (menu_dBper_value[item] == get_trace_scale(1)){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
   } else if (menu == menu_rbw) {
     if (rbwsel[item] == GetRBW()){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
 
-  } else if (menu == menu_storage) {
-    if (item ==0 && GetStorage()){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+  } else if (menu == menu_drive || menu == menu_drive_wide) {
+    if (item == setting_drive){
+      mark = true;
     }
-    if (item == 2 && GetSubtractStorage()){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+
+  } else if (menu == menu_display) {
+    if (item ==2 && GetStorage()){
+      mark = true;
     }
-    if (item == 3 && get_waterfall()){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+    if (item == 4 && GetSubtractStorage()){
+      mark = true;
+    }
+    if (item == 5 && get_waterfall()){
+      mark = true;
     }
   } else if (menu == menu_settings2 || menu == menu_settingshigh2) {
     if (item ==0 && setting_agc){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
     if (item == 1 && setting_lna){
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
     if (item == 2 && setting_tracking){         // should not happen in high mode
-      *bg = DEFAULT_MENU_TEXT_COLOR;
-      *fg = config.menu_normal_color;
+      mark = true;
     }
+  } else if (menu == menu_marker_type && active_marker >= 0 && markers[active_marker].enabled) {
+    if (item == 3 && markers[active_marker].enabled == M_TRACKING_ENABLED)
+      mark = true;
+    else if (item == markers[active_marker].mtype)
+      mark = true;
+  } else if (menu == menu_marker_sel) {
+    if (item < MARKERS_MAX && markers[item].enabled)
+      mark = true;
+  }
+  if (mark) {
+    *bg = DEFAULT_MENU_TEXT_COLOR;
+    *fg = config.menu_normal_color;
   }
   if (ui_mode == UI_MENU && menu_is_form(menu)) {
     //    if (item == 0)
     //      redraw_frame();
     if (item <= 1) {
-
       area_width = 0;
-#if 0
-      //    area_height = HEIGHT - 32;
-      int y = MENU_BUTTON_HEIGHT*item;
-      uint16_t bg = config.menu_normal_color;
-      uint16_t fg = DEFAULT_MENU_TEXT_COLOR;
-      //    ili9341_fill(320-MENU_BUTTON_WIDTH, y, MENU_BUTTON_WIDTH, MENU_BUTTON_HEIGHT-2, bg);
-      ili9341_set_foreground(fg);
-      ili9341_set_background(bg);
-      char buf[15];
-      ili9341_fill(50+25, y, 170, MENU_BUTTON_HEIGHT-2, bg);
-      if (menu == menu_lowoutputmode) {
-        switch (item) {
-        case 0:
-          set_sweep_frequency(ST_SPAN, 0);          // For CW sweep mode
-          plot_printf(buf, sizeof buf, "%3.3fMHz", frequency0 / 1000000.0);
-          break;
-        case 1:
-          plot_printf(buf, sizeof buf, "%ddB", -10 - setting_attenuate);
-          break;
-        }
-      }
-      if (menu == menu_highoutputmode) {
-        switch (item) {
-        case 0:
-          set_sweep_frequency(ST_SPAN, 0);          // For CW sweep mode
-          plot_printf(buf, sizeof buf, "%3.3fMHz", frequency0 / 1000000.0);
-          break;
-        case 1:
-          plot_printf(buf, sizeof buf, "%ddB", -10 - setting_drive);
-          break;
-        }
-      }
-      ili9341_drawstring_size(buf, 130, y+6, 2);
-#endif
     }
   }else{
     area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;