diff --git a/ili9341.c b/ili9341.c
index a1942ab..0cd5ad2 100644
--- a/ili9341.c
+++ b/ili9341.c
@@ -384,39 +384,6 @@ void ili9341_bulk(int x, int y, int w, int h)
   }
 }
 #else
-static uint8_t ssp_sendrecvdata(void)
-{
-  // Start RX clock (by sending data)
-  SPI_WRITE_8BIT(0);
-  while (SPI_RX_IS_EMPTY && SPI_IS_BUSY)
-    ;
-  return SPI_READ_DATA;
-}
-
-void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
-{
-  // uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
-  // uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
-  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
-  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
-  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
-  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
-  send_command(ILI9341_MEMORY_READ, 0, NULL);
-
-  // Skip data from rx buffer
-  while (SPI_RX_IS_NOT_EMPTY)
-    (void) SPI_READ_DATA;
-  // require 8bit dummy clock
-  ssp_sendrecvdata();
-  while (len-- > 0) {
-    // read data is always 18bit
-    uint8_t r = ssp_sendrecvdata();
-    uint8_t g = ssp_sendrecvdata();
-    uint8_t b = ssp_sendrecvdata();
-    *out++ = RGB565(r, g, b);
-  }
-  CS_HIGH;
-}
 
 //
 // Use DMA for send data
@@ -469,7 +436,7 @@ void ili9341_bulk(int x, int y, int w, int h)
                               STM32_DMA_CR_MSIZE_HWORD | STM32_DMA_CR_MINC);
   dmaStreamFlush(w * h);
 }
-#if 0  // Read DMA hangs
+#if 1  // Read DMA hangs
 // Copy screen data to buffer
 // Warning!!! buffer size must be greater then 3*len + 1 bytes
 void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
@@ -515,6 +482,41 @@ void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
     rgbbuf += 3;
   }
 }
+#else
+static uint8_t ssp_sendrecvdata(void)
+{
+  // Start RX clock (by sending data)
+  SPI_WRITE_8BIT(0);
+  while (SPI_RX_IS_EMPTY && SPI_IS_BUSY)
+    ;
+  return SPI_READ_DATA;
+}
+
+void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
+{
+  // uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
+  // uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
+  send_command(ILI9341_MEMORY_READ, 0, NULL);
+
+  // Skip data from rx buffer
+  while (SPI_RX_IS_NOT_EMPTY)
+    (void) SPI_READ_DATA;
+  // require 8bit dummy clock
+  ssp_sendrecvdata();
+  while (len-- > 0) {
+    // read data is always 18bit
+    uint8_t r = ssp_sendrecvdata();
+    uint8_t g = ssp_sendrecvdata();
+    uint8_t b = ssp_sendrecvdata();
+    *out++ = RGB565(r, g, b);
+  }
+  CS_HIGH;
+}
+
 #endif
 #endif
 
@@ -590,6 +592,16 @@ void ili9341_drawstring(const char *str, int x, int y)
   }
 }
 
+void ili9341_drawstring_7x13(const char *str, int x, int y)
+{
+  while (*str) {
+    uint8_t ch = *str++;
+    const uint16_t *char_buf = &x7x13b_bits[(ch * 13)];
+    blit16BitWidthBitmap(x, y, 7, 11, char_buf);
+    x += 7;
+  }
+}
+
 void ili9341_drawstringV(const char *str, int x, int y)
 {
   ili9341_set_rotation(DISPLAY_ROTATION_270);
diff --git a/main.c b/main.c
index 8a4007f..424b80c 100644
--- a/main.c
+++ b/main.c
@@ -615,7 +615,16 @@ int16_t dump_buffer[AUDIO_BUFFER_LEN];
 int16_t dump_selection = 0;
 #endif
 
-volatile int16_t wait_count = 0;
+volatile uint8_t wait_count = 0;
+volatile uint8_t accumerate_count = 0;
+
+const int8_t bandwidth_accumerate_count[] = {
+  1, // 1kHz
+  3, // 300Hz
+  10, // 100Hz
+  33, // 30Hz
+  100 // 10Hz
+};
 
 float measured[2][POINTS_COUNT][2];
 #endif
@@ -643,13 +652,16 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
   (void)i2sp;
   (void)n;
 
-  if (wait_count > 0) {
-    if (wait_count == 1) 
+  if (wait_count > 1) {
+    --wait_count;
+  } else if (wait_count > 0) {
+    if (accumerate_count > 0) {
       dsp_process(p, n);
+      accumerate_count--;
+    }
 #ifdef ENABLED_DUMP
-      duplicate_buffer_to_dump(p);
+    duplicate_buffer_to_dump(p);
 #endif
-    --wait_count;
   }
 
 #if PORT_SUPPORTS_RT
@@ -786,8 +798,8 @@ config_t config = {
   .harmonic_freq_threshold = 300000000,
 #endif
   .vbat_offset = 500,
-  .low_level_offset =       0,
-  .high_level_offset =      0,
+  .low_level_offset =       100,    // Uncalibrated
+  .high_level_offset =      100,    // Uncalibrated
 };
 
 properties_t current_props;
@@ -801,7 +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_DELTA, 40, 0 }, { 0, M_DELTA, 60, 0 }, { 0, M_DELTA, 80, 0 }
+    { 1, M_REFERENCE, 30, 0 },
+    { 0, M_NORMAL, 40, 0 },
+    { 0, M_NORMAL, 60, 0 },
+    { 0, M_NORMAL, 80, 0 }
 };
 
 // Load propeties default settings
@@ -868,20 +883,20 @@ bool sweep(bool break_on_operation)
     if (frequencies[i] == 0) break;
     delay = set_frequency(frequencies[i]);     // 700
     tlv320aic3204_select(0);                   // 60 CH0:REFLECT, reset and begin measure
-    DSP_START(delay + ((i == 0) ? 1 : 0));     // 1900
+    dsp_start(delay + ((i == 0) ? 1 : 0));     // 1900
     //================================================
     // Place some code thats need execute while delay
     //================================================
-    DSP_WAIT_READY;
+    dsp_wait();
     // calculate reflection coefficient
     (*sample_func)(measured[0][i]);            // 60
 
     tlv320aic3204_select(1);                   // 60 CH1:TRANSMISSION, reset and begin measure
-    DSP_START(DELAY_CHANNEL_CHANGE);           // 1700
+    dsp_start(DELAY_CHANNEL_CHANGE);           // 1700
     //================================================
     // Place some code thats need execute while delay
     //================================================
-    DSP_WAIT_READY;
+    dsp_wait();
     // calculate transmission coefficient
     (*sample_func)(measured[1][i]);            // 60
                                                // ======== 170 ===========
@@ -995,7 +1010,8 @@ set_frequencies(uint32_t start, uint32_t stop, uint16_t points)
   // disable at out of sweep range
   for (; i < POINTS_COUNT; i++)
     frequencies[i] = 0;
-  update_rbw(frequencies[1] - frequencies[0]);
+  setting_frequency_step = delta;
+  update_rbw();
 }
 
 static void
@@ -2226,7 +2242,7 @@ VNA_SHELL_FUNCTION(cmd_d)
 {
   (void) argc;
   int32_t a = my_atoi(argv[0]);
-  settingDrive = a;
+  setting_drive = a;
 }
 
 
@@ -2253,11 +2269,11 @@ VNA_SHELL_FUNCTION(cmd_t)
 VNA_SHELL_FUNCTION(cmd_e)
 {
   (void)argc;
-  extraVFO = my_atoi(argv[0]);
-  if (extraVFO == -1)
-    extraVFO = false;
+  setting_tracking = my_atoi(argv[0]);
+  if (setting_tracking == -1)
+    setting_tracking = false;
   else
-    extraVFO = true;
+    setting_tracking = true;
 
   if (argc >1)
     frequencyExtra = my_atoi(argv[1]);
@@ -2276,11 +2292,12 @@ VNA_SHELL_FUNCTION(cmd_m)
   pause_sweep();
   int32_t f_step = (frequencyStop-frequencyStart)/ points;
   palClearPad(GPIOC, GPIOC_LED);  // disable led and wait for voltage stabilization
-  update_rbw(f_step);
+  setting_frequency_step = f_step;
+  update_rbw();
   chThdSleepMilliseconds(10);
   streamPut(shell_stream, '{');
   for (int i = 0; i<points; i++) {
-      float val = perform(false, i, frequencyStart - frequency_IF + f_step * i, extraVFO);
+      float val = perform(false, i, frequencyStart - frequency_IF + f_step * i, setting_tracking);
       streamPut(shell_stream, 'x');
       int v = val*2 + 256;
       streamPut(shell_stream, (uint8_t)(v & 0xFF));
diff --git a/nanovna.h b/nanovna.h
index 93398a9..276df10 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -33,14 +33,18 @@
 #define POINTS_COUNT     290
 #define MARKER_COUNT    4
 
-#define TRACE_COUNT     3
+#define TRACES_MAX 3
+#define TRACE_AGE       3
 #define TRACE_ACTUAL    2
 #define TRACE_STORED    1
 #define TRACE_TEMP      0
+// #define age_t     measured[TRACE_AGE]
 #define stored_t  measured[TRACE_STORED]
 #define actual_t  measured[TRACE_ACTUAL]
 #define temp_t    measured[TRACE_TEMP]
-typedef float measurement_t[3][POINTS_COUNT];
+
+
+typedef float measurement_t[TRACES_MAX][POINTS_COUNT];
 extern measurement_t measured;
 #endif
 
@@ -117,11 +121,22 @@ void load_default_properties(void);
 
 extern float perform(bool b, int i, int32_t f, int e);
 enum {
-  AV_OFF, AV_MIN, AV_MAX, AV_2, AV_4, AV_8
+  AV_OFF, AV_MIN, AV_MAX_HOLD, AV_MAX_DECAY, AV_4, AV_16
 };
 enum {
   M_LOW, M_HIGH, M_GENLOW, M_GENHIGH,
 };
+
+enum {
+  MO_NONE, MO_AM, MO_NFM, MO_WFM,
+};
+
+#define MODE_OUTPUT(x)  ((x) == M_GENLOW || (x) == M_GENHIGH )
+#define MODE_INPUT(x)  ((x) == M_LOW || (x) == M_HIGH )
+#define MODE_HIGH(x)  ((x) == M_HIGH || (x) == M_GENHIGH )
+#define MODE_LOW(x)  ((x) == M_LOW || (x) == M_GENLOW )
+#define MODE_SELECT(x) (MODE_HIGH(x) ? 1 : 0)
+
 #define SWEEP_ENABLE  0x01
 #define SWEEP_ONCE    0x02
 extern int8_t sweep_mode;
@@ -232,8 +247,6 @@ extern const uint16_t numfont16x22[];
 #define S_OHM   "\036"
 // trace 
 
-#define TRACES_MAX 3
-
 #define MAX_TRACE_TYPE 12
 enum trace_type {
   TRC_LOGMAG=0, TRC_PHASE, TRC_DELAY, TRC_SMITH, TRC_POLAR, TRC_LINEAR, TRC_SWR, TRC_REAL, TRC_IMAG, TRC_R, TRC_X, TRC_OFF
@@ -294,6 +307,7 @@ void set_trace_refpos(int t, float refpos);
 float get_trace_scale(int t);
 float get_trace_refpos(int t);
 const char *get_trace_typename(int t);
+extern int in_selftest;
 
 #ifdef __VNA
 void set_electrical_delay(float picoseconds);
@@ -364,6 +378,8 @@ extern volatile uint8_t redraw_request;
 
 #define DEFAULT_FG_COLOR            RGB565(255,255,255)
 #define DEFAULT_BG_COLOR            RGB565(  0,  0,  0)
+#define DARK_GREY                   RGB565(140,140,140)
+#define LIGHT_GREY                  RGB565(220,220,220)
 #define DEFAULT_GRID_COLOR          RGB565(128,128,128)
 #define DEFAULT_MENU_COLOR          RGB565(255,255,255)
 #define DEFAULT_MENU_TEXT_COLOR     RGB565(  0,  0,  0)
@@ -394,6 +410,7 @@ 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);
 void ili9341_drawstring(const char *str, int x, int y);
+void ili9341_drawstring_7x13(const char *str, int x, int y);
 void ili9341_drawstringV(const char *str, int x, int y);
 int  ili9341_drawchar_size(uint8_t ch, int x, int y, uint8_t size);
 void ili9341_drawstring_size(const char *str, int x, int y, uint8_t size);
@@ -407,7 +424,7 @@ void show_logo(void);
  * flash.c
  */
 
-#if 0
+#if 1
 #define SAVEAREA_MAX 5
 // Begin addr                   0x08018000
 #define SAVE_CONFIG_AREA_SIZE   0x00008000
@@ -456,6 +473,7 @@ typedef struct properties {
 #ifdef __VNA__
   uint8_t _domain_mode; /* 0bxxxxxffm : where ff: TD_FUNC m: DOMAIN_MODE */
   uint8_t _marker_smith_format;
+  uint8_t _bandwidth;
 #endif  
   uint8_t _reserved[2];
   uint32_t checksum;
@@ -490,6 +508,7 @@ extern properties_t current_props;
 #define domain_mode current_props._domain_mode
 #define velocity_factor current_props._velocity_factor
 #define marker_smith_format current_props._marker_smith_format
+#define bandwidth current_props._bandwidth
 #endif
 
 #define FREQ_IS_STARTSTOP() (!(config.freq_mode&FREQ_MODE_CENTER_SPAN))
@@ -510,6 +529,7 @@ void clear_all_config_prop_data(void);
  */
 extern void ui_init(void);
 extern void ui_process(void);
+int current_menu_is_form(void);
 
 // Irq operation process set
 #define OP_NONE       0x00
@@ -537,6 +557,7 @@ typedef struct uistat {
   uint8_t lever_mode;
   uint8_t marker_delta;
   uint8_t marker_tracking;
+  char text[20];
 } uistat_t;
 
 extern uistat_t uistat;
@@ -570,12 +591,13 @@ int16_t adc_vbat_read(void);
  */
 int plot_printf(char *str, int, const char *fmt, ...);
 #define PULSE do { palClearPad(GPIOC, GPIOC_LED); palSetPad(GPIOC, GPIOC_LED);} while(0)
-extern int settingAttenuate;
+extern int setting_attenuate;
 extern int settingPowerCal;
-extern int stepDelay;
-extern int settingSpeed;
-extern int settingMode;
-void update_rbw(uint32_t delta_f);
+extern int setting_step_delay;
+extern int actualStepDelay;
+extern int setting_mode;
+void update_rbw(void);
+int GetActualRBW(void);
 
 #define byte uint8_t
 extern volatile int SI4432_Sel;         // currently selected SI4432
@@ -590,7 +612,7 @@ void SI4432_SetReference(int freq);
 
 // Speed profile definition
 #define START_PROFILE   systime_t time = chVTGetSystemTimeX();
-#define STOP_PROFILE    {char string_buf[12];plot_printf(string_buf, sizeof string_buf, "T:%06d", chVTGetSystemTimeX() - time);ili9341_drawstringV(string_buf, 1, 60);}
+#define STOP_PROFILE    {char string_buf[12];plot_printf(string_buf, sizeof string_buf, "T:%06d", chVTGetSystemTimeX() - time);ili9341_drawstringV(string_buf, 1, 180);}
 // Macros for convert define value to string
 #define STR1(x)  #x
 #define define_to_STR(x)  STR1(x)
@@ -600,6 +622,11 @@ int GetRBW(void);
 int GetStorage(void);
 int GetSubtractStorage(void);
 int get_waterfall(void);
-
-
+void toggle_tracking(void);
+void calibrate(void);
+void reset_calibration(void);
+void SetRefpos(int);
+void SetScale(int);
+void SetRBW(int);
+void SetRX(int);
 /*EOF*/
diff --git a/plot.c b/plot.c
index 31a7771..56748a3 100644
--- a/plot.c
+++ b/plot.c
@@ -1,5 +1,4 @@
-/*
- * Copyright (c) 2014-2015, TAKAHASHI Tomohiro (TTRFTECH) edy555@gmail.com
+/* Copyright (c) 2014-2015, TAKAHASHI Tomohiro (TTRFTECH) edy555@gmail.com
  * All rights reserved.
  *
  * This is free software; you can redistribute it and/or modify
@@ -33,7 +32,7 @@ int fullscreen = true;
 static void cell_draw_marker_info(int x0, int y0);
 static void draw_battery_status(void);
 void cell_draw_test_info(int x0, int y0);
-static void frequency_string(char *buf, size_t len, int32_t freq);
+void frequency_string(char *buf, size_t len, int32_t freq);
 
 static int16_t grid_offset;
 static int16_t grid_width;
@@ -68,7 +67,7 @@ typedef uint32_t map_t;
 uint16_t marker_color[3] =
 {
  RGBHEX(0xFFFFFF),
- RGBHEX(0x0000FF),
+ RGBHEX(0xFFFF00),
  RGBHEX(0x00FF00)
 };
 
@@ -796,12 +795,19 @@ static void trace_get_value_string(
   } else {
     dfreq = frequencies[i];
   }
-  frequency_string(&buf2[1], sizeof(buf2) -1, dfreq);
+  if (GetActualRBW()  < 10)
+    plot_printf(&buf2[1], sizeof(buf2) -1, "%3.3f" , (dfreq + 500) / 1000000.0);
+  else if (GetActualRBW()  < 100)
+    plot_printf(&buf2[1], sizeof(buf2) -1, "%3.2f" , (dfreq + 5000) / 1000000.0);
+  else
+    plot_printf(&buf2[1], sizeof(buf2) -1, "%3.1f" , (dfreq + 50000) / 1000000.0);
+
+//  frequency_string(&buf2[1], sizeof(buf2) -1, dfreq);
     v = logmag(&coeff[i]);
     if (v == -INFINITY)
       plot_printf(buf, len, "-INF");
     else
-      plot_printf(buf, len, " %s %.2f", buf2, v - rlevel);
+      plot_printf(buf, len, "%s %.1f", buf2, v - rlevel);
 }
 #ifdef __VNA__
 static int
@@ -1094,7 +1100,7 @@ markmap_marker(int marker)
   int t;
   if (!markers[marker].enabled)
     return;
-  for (t = 0; t < TRACES_MAX; t++) {
+  for (t = TRACE_ACTUAL; t < TRACE_ACTUAL; t++) {
     if (!trace[t].enabled)
       continue;
     index_t index = trace_index[t][markers[marker].index];
@@ -1138,9 +1144,9 @@ marker_search(void)
   if (uistat.current_trace == -1)
     return -1;
 
-  int value = CELL_Y(trace_index[uistat.current_trace][0]);
+  int value = CELL_Y(trace_index[TRACE_ACTUAL][0]);
   for (i = 0; i < sweep_points; i++) {
-    index_t index = trace_index[uistat.current_trace][i];
+    index_t index = trace_index[TRACE_ACTUAL][i];
     if ((*compare)(value, CELL_Y(index))) {
       value = CELL_Y(index);
       found = i;
@@ -1161,21 +1167,21 @@ marker_search_left(int from)
 {
   int i;
   int found = -1;
-
+#define MINMAX_DELTA    -10
   if (uistat.current_trace == -1)
     return -1;
 
-  int value = CELL_Y(trace_index[uistat.current_trace][from]);
+  int value = CELL_Y(trace_index[TRACE_ACTUAL][from]);
   for (i = from - 1; i >= 0; i--) {
-    index_t index = trace_index[uistat.current_trace][i];
-    if ((*compare)(value, CELL_Y(index)))
+    index_t index = trace_index[TRACE_ACTUAL][i];
+    if ((*compare)(value - MINMAX_DELTA, CELL_Y(index)))
       break;
     value = CELL_Y(index);
   }
 
   for (; i >= 0; i--) {
-    index_t index = trace_index[uistat.current_trace][i];
-    if ((*compare)(CELL_Y(index), value)) {
+    index_t index = trace_index[TRACE_ACTUAL][i];
+    if ((*compare)(CELL_Y(index), value + MINMAX_DELTA)) {
       break;
     }
     found = i;
@@ -1192,17 +1198,16 @@ marker_search_right(int from)
 
   if (uistat.current_trace == -1)
     return -1;
-
-  int value = CELL_Y(trace_index[uistat.current_trace][from]);
+  int value = CELL_Y(trace_index[TRACE_ACTUAL][from]);
   for (i = from + 1; i < sweep_points; i++) {
-    index_t index = trace_index[uistat.current_trace][i];
+    index_t index = trace_index[TRACE_ACTUAL][i];
     if ((*compare)(value, CELL_Y(index)))
       break;
     value = CELL_Y(index);
   }
 
   for (; i < sweep_points; i++) {
-    index_t index = trace_index[uistat.current_trace][i];
+    index_t index = trace_index[TRACE_ACTUAL][i];
     if ((*compare)(CELL_Y(index), value)) {
       break;
     }
@@ -1385,9 +1390,10 @@ draw_cell(int m, int n)
   for (i = 0; i < MARKERS_MAX; i++) {
     if (!markers[i].enabled)
       continue;
-    for (t = 0; t < TRACES_MAX; t++) {
-      if (!trace[t].enabled)
-        continue;
+//    for (t = 0; t < TRACES_MAX; t++) {
+//      if (!trace[t].enabled)
+//        continue;
+      t = TRACE_ACTUAL;
       index_t index = trace_index[t][markers[i].index];
       int x = CELL_X(index) - x0 - X_MARKER_OFFSET;
       int y = CELL_Y(index) - y0 - Y_MARKER_OFFSET;
@@ -1396,7 +1402,7 @@ draw_cell(int m, int n)
           y + MARKER_HEIGHT >= 0 && y - MARKER_HEIGHT < CELLHEIGHT)
         draw_marker(x, y, marker_color[markers[i].mtype], i);
 //      draw_marker(x, y, config.trace_color[t], i);
-    }
+//    }
   }
 #endif
 // Draw trace and marker info on the top (50 system ticks for all screen calls)
@@ -1497,7 +1503,7 @@ draw_all_cells(bool flush_markmap)
 #endif
       spi_buffer[i] = RGB565(r,g,b);
     }
-    ili9341_bulk(5*5,HEIGHT+3, 290,1);
+    ili9341_bulk(5*5,HEIGHT+3, area_width,1);
   }
 #endif
 }
@@ -1817,31 +1823,40 @@ static void cell_draw_marker_info(int x0, int y0)
         continue;
 #if 1
       int xpos = 1 + (j%2)*(WIDTH/2) + CELLOFFSETX - x0;
-      int ypos = 1 + (j/2)*(13) - y0;
+//      int ypos = 1 + (j/2)*(13) - y0;
+      int ypos = 1 + (j/2)*(16) - y0;
 #else
       int xpos = 1 + CELLOFFSETX - x0;
       int ypos = 1 + j*(FONT_GET_HEIGHT*2+1) - y0;
 #endif
       int k = 0;
-      if (i == active_marker)
+      if (i == active_marker) {
+//        ili9341_set_foreground(DEFAULT_BG_COLOR);
+//        ili9341_set_background(marker_color[markers[i].mtype]);
         buf[k++] = '\033'; // Right arrow (?)
-      else
+      } else {
+//        ili9341_set_background(DEFAULT_BG_COLOR);
+//        ili9341_set_foreground(marker_color[markers[i].mtype]);
         buf[k++] = ' ';
+//        buf[k++] = ' ';
+      }
       buf[k++] = i+'1';
-      buf[k++] = marker_letter[markers[i].mtype];
+//      buf[k++] = marker_letter[markers[i].mtype];
       buf[k++] = 0;
+      ili9341_set_background(DEFAULT_BG_COLOR);
       ili9341_set_foreground(marker_color[markers[i].mtype]);
       cell_drawstring_7x13(buf, xpos, ypos);
+//      cell_drawstring_size(buf, xpos, ypos, 2);
       trace_get_value_string(
           t, buf, sizeof buf,
           idx, measured[trace[t].channel], frequencies, sweep_points, ridx, markers[i].mtype);
-//      cell_drawstring_7x13(w, h, buf, xpos+2*7, ypos, config.trace_color[t]);
-      cell_drawstring_7x13(buf, xpos+4*7, ypos);
+      cell_drawstring_7x13(buf, xpos+3*7, ypos);
+//      cell_drawstring_size(buf, xpos+3*7, ypos, 2);
       j++;
    }
   }
 }
-static void frequency_string(char *buf, size_t len, int32_t freq)
+void frequency_string(char *buf, size_t len, int32_t freq)
 {
   if (freq < 0) {
     freq = -freq;
@@ -1885,6 +1900,11 @@ draw_frequencies(void)
 {
   char buf1[32];
   char buf2[32]; buf2[0] = 0;
+  if (MODE_OUTPUT(setting_mode))     // No frequencies during output
+    return;
+  if (current_menu_is_form() && !in_selftest)
+    return;
+
 #ifdef __VNA__
   if ((domain_mode & DOMAIN_MODE) == DOMAIN_FREQ) {
 #endif
diff --git a/sa_core.c b/sa_core.c
index d870670..646bf2e 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -2,136 +2,128 @@
 
 #include "SI4432.h"		// comment out for simulation
 
-#if 0
-//-----------------SI4432 dummy------------------
-void SI4432_Write_Byte(unsigned char ADR, unsigned char DATA ) {}
-unsigned char SI4432_Read_Byte(unsigned char ADR) {return ADR;}
-float SI4432_SET_RBW(float WISH) {return (WISH > 600.0?600: (WISH<3.0?3:WISH));}
-void SI4432_SetReference(int p) {}
-void SI4432_Set_Frequency(long f) {}
-void PE4302_Write_Byte(unsigned char DATA ) {}
-void PE4302_init(void) {}
-#endif
-
-#ifdef __SIMULATION__
-unsigned long seed = 123456789;
-extern float rbw;
-float myfrand(void)
-{
-  seed = (unsigned int) (1103515245 * seed + 12345) ;
-  return ((float) seed) / 1000000000.0;
-}
-#define NOISE  ((myfrand()-2) * 2)  // +/- 4 dBm noise
-extern int settingAttenuate;
-
-//#define LEVEL(i, f, v) (v * (1-(fabs(f - frequencies[i])/rbw/1000)))
-
-float LEVEL(uint32_t i, uint32_t f, int v)
-{
-  float dv;
-  float df = fabs((float)f - (float)i);
-  if (df < rbw*1000)
-    dv = df/(rbw*1000);
-  else
-    dv =  1 + 50*(df - rbw*1000)/(rbw*1000);
-  return (v - dv - settingAttenuate);
-}
+int setting_mode = M_LOW;
 
-float Simulated_SI4432_RSSI(uint32_t i, int s)
-{
-  SI4432_Sel = s;
-  float v = -100 + log10(rbw)*10 + NOISE;
-  if(s == 0) {
-    v = fmax(LEVEL(i,10000000,-20),v);
-    v = fmax(LEVEL(i,20000000,-40),v);
-    v = fmax(LEVEL(i,30000000,-30),v);
-    v = fmax(LEVEL(i,40000000,-90),v);
-  } else {
-    v = fmax(LEVEL(i,320000000,-20),v);
-    v = fmax(LEVEL(i,340000000,-40),v);
-    v = fmax(LEVEL(i,360000000,-30),v);
-    v = fmax(LEVEL(i,380000000,-90),v);
-  }
-  return(v);
-}
-
-#endif
-//--------------------- Frequency control -----------------------
 
 int dirty = true;
 int scandirty = true;
+int setting_attenuate = 0;
+int setting_rbw = 0;
+int setting_average = 0;
+int setting_show_stored = 0;
+int setting_subtract_stored = 0;
+int setting_drive=0; // 0-3 , 3=+20dBm
+int setting_agc = true;
+int setting_lna = false;
+int setting_tracking = false;
+int setting_modulation = MO_NONE;
+int setting_step_delay = 0;
+int setting_frequency_step;
+float actual_rbw = 0;
+float setting_vbw = 0;
 
-//---------------- menu system -----------------------
+int vbwSteps = 1;
 
-int settingAttenuate = 0;
-int settingGenerate = 0;
-int settingBandwidth = 0;
+//int setting_spur = 0;
+uint32_t minFreq = 0;
+uint32_t maxFreq = 520000000;
 
-//int settingLevelOffset = 0;
+int setting_refer = -1;  // Off by default
+const int reffer_freq[] = {30000000, 15000000, 10000000, 4000000, 3000000, 2000000, 1000000};
 
-int settingRefer = 1;
-int refferFreq[] = {30000000, 15000000, 10000000, 4000000, 3000000, 2000000, 1000000};
-int settingSpur = 0;
-int settingAverage = 0;
-int settingShowStorage = 0;
-int settingSubtractStorage = 0;
-int settingMode = 0;
-int settingDrive=0; // 0-3 , 3=+20dBm
-int settingAGC = true;
-int settingLNA = false;
-int extraVFO = false;
+int in_selftest = false;
 
-uint32_t minFreq = 0;
-uint32_t maxFreq = 520000000;
+void reset_settings(int m)
+{
+  setting_mode = m;
+  setting_attenuate = 0;
+  setting_rbw = 0;
+  setting_average = 0;
+  setting_show_stored = 0;
+  setting_subtract_stored = 0;
+  setting_drive=0; // 0-3 , 3=+20dBm
+  setting_agc = true;
+  setting_lna = false;
+  setting_tracking = false;
+  setting_modulation = MO_NONE;
+  setting_step_delay = 0;
+  setting_vbw = 0;
+//  setting_spur = 0;
+  switch(m) {
+  case M_LOW:
+    minFreq = 0;
+    maxFreq = 520000000;
+    set_sweep_frequency(ST_START, (int32_t) 0);
+    set_sweep_frequency(ST_STOP, (int32_t) 350000000);
+    SetRefpos(-10);
+    break;
+  case M_GENLOW:
+    minFreq = 0;
+    maxFreq = 520000000;
+    set_sweep_frequency(ST_CENTER, (int32_t) 10000000);
+    set_sweep_frequency(ST_SPAN, 0);
+    break;
+  case M_HIGH:
+    minFreq = 240000000;
+    maxFreq = 960000000;
+    set_sweep_frequency(ST_START, (int32_t) minFreq);
+    set_sweep_frequency(ST_STOP, (int32_t) maxFreq);
+    SetRefpos(-30);
+    break;
+  case M_GENHIGH:
+    minFreq = 240000000;
+    maxFreq = 960000000;
+    set_sweep_frequency(ST_CENTER, (int32_t) 300000000);
+    set_sweep_frequency(ST_SPAN, 0);
+    break;
+  }
+  SetScale(10);
+  dirty = true;
+}
 
 void set_refer_output(int v)
 {
-  settingRefer = v;
+  setting_refer = v;
   dirty = true;
 }
 
 int get_refer_output(void)
 {
-  return(settingRefer);
+  return(setting_refer);
 }
 
-void SetGenerate(int g)
+void SetDrive(int d)
 {
-  settingGenerate = g;
+  setting_drive = d;
   dirty = true;
 }
 
-int GetMode(void)
+void SetModulation(int m)
 {
-  return(settingMode);
+  setting_modulation = m;
+  dirty = true;
 }
-
-void SetMode(int m)
+void SetIF(int f)
 {
-  settingMode = m;
-  switch(m) {
-  case M_LOW:
-  case M_GENLOW:
-    minFreq = 0;
-    maxFreq = 520000000;
-    set_sweep_frequency(ST_START, (int32_t) 0);
-    set_sweep_frequency(ST_STOP, (int32_t) 300000000);
-    break;
-  case M_HIGH:
-  case M_GENHIGH:
-    minFreq = 260000000;
-    maxFreq = 960000000;
-    set_sweep_frequency(ST_START, (int32_t) 300000000);
-    set_sweep_frequency(ST_STOP, (int32_t) 960000000);
-    break;
-  }
+  frequency_IF = f;
   dirty = true;
 }
 
+int GetMode(void)
+{
+  return(setting_mode);
+  dirty = true;
+}
 
 void SetAttenuation(int a)
 {
-  settingAttenuate = a;
+  if (a<0)
+    a = 0;
+  if (a> 31)
+    a=31;
+  if (setting_attenuate == a)
+    return;
+  setting_attenuate = a;
   dirty = true;
 }
 
@@ -139,324 +131,223 @@ void SetStorage(void)
 {
   for (int i=0; i<POINTS_COUNT;i++)
     stored_t[i] = actual_t[i];
-  settingShowStorage = true;
+  setting_show_stored = true;
   trace[TRACE_STORED].enabled = true;
+  dirty = true;
 }
 
 int GetStorage(void)
 {
-  return(settingShowStorage);
+  return(setting_show_stored);
 }
 
 void SetClearStorage(void)
 {
-  settingShowStorage = false;
-  settingSubtractStorage = false;
+  setting_show_stored = false;
+  setting_subtract_stored = false;
   trace[TRACE_STORED].enabled = false;
   dirty = true;
 }
 
 void SetSubtractStorage(void)
 {
-  if (!settingSubtractStorage) {
-    if (!settingShowStorage)
+  if (!setting_subtract_stored) {
+    if (!setting_show_stored)
       SetStorage();
-    settingSubtractStorage = true;
+    setting_subtract_stored = true;
   } else {
-    settingSubtractStorage = false;
+    setting_subtract_stored = false;
   }
   dirty = true;
 }
 
 int GetSubtractStorage(void)
 {
-  return(settingSubtractStorage);
+  return(setting_subtract_stored);
 }
 
 extern float peakLevel;
 void SetPowerLevel(int o)
 {
+  float new_offset = o - peakLevel - setting_attenuate + settingLevelOffset();
   if (o != 100) {
-    if (settingMode & 1)
-      config.high_level_offset = o - peakLevel - settingAttenuate + settingLevelOffset();
-    else
-      config.low_level_offset = o - peakLevel - settingAttenuate + settingLevelOffset();
+    if (setting_mode == M_HIGH)
+      config.high_level_offset = new_offset;
+    else if (setting_mode == M_LOW)
+      config.low_level_offset = new_offset;
   }
   else {
-    config.low_level_offset = 0;
-    config.high_level_offset = 0;
+    config.low_level_offset = 100;
+    config.high_level_offset = 100;
   }
   dirty = true;
 }
 
 int settingLevelOffset(void)
 {
-  if (settingMode & 1)
+  if (setting_mode == M_HIGH) {
+    if (config.high_level_offset == 100)
+      return 0;
     return(config.high_level_offset);
-  return(config.low_level_offset);
+  }
+  if (setting_mode == M_LOW) {
+    if (config.low_level_offset == 100)
+      return 0;
+    return(config.low_level_offset);
+  }
+  return(0);
+}
+
+int level_is_calibrated(void)
+{
+  if (setting_mode == M_HIGH && config.high_level_offset != 100)
+    return 1;
+  if (setting_mode == M_LOW && config.low_level_offset != 100)
+    return 1;
+  return(0);
 }
 
 void SetRBW(int v)
 {
-  settingBandwidth = v;
-  update_rbw(frequencies[1] - frequencies[0]);
+  setting_rbw = v;
+  update_rbw();
   dirty = true;
 }
 
 int GetRBW(void)
 {
-  return(settingBandwidth);
+  return(setting_rbw);
 }
 
+int GetActualRBW(void)
+{
+  return((int) actual_rbw);
+}
+#if 0
 void SetSpur(int v)
 {
-  settingSpur = v;
+//  setting_spur = v;
   dirty = true;
 }
+#endif
 
-int GetSpur(void)
+void SetStepDelay(int d)
 {
-  return(settingSpur);
+  setting_step_delay = d;
+  dirty = true;
 }
 
 void SetAverage(int v)
 {
-  settingAverage = v;
+  setting_average = v;
   trace[TRACE_TEMP].enabled = (v != 0);
   dirty = true;
 }
 
 int GetAverage(void)
 {
-  return(settingAverage);
+  return(setting_average);
 }
 
 void ToggleLNA(void)
 {
-  settingLNA = !settingLNA;
+  setting_lna = !setting_lna;
+  dirty = true;
+}
+
+void toggle_tracking(void)
+{
+  setting_tracking = !setting_tracking;
+  dirty = true;
+}
+
+int GetExtraVFO(void)
+{
+  return(setting_tracking);
 }
 
 int GetLNA(void)
 {
-  return(settingLNA);
+  return(setting_lna);
 }
 
 void ToggleAGC(void)
 {
-  settingAGC = !settingAGC;
+  setting_agc = !setting_agc;
+  dirty = true;
 }
 
 int GetAGC(void)
 {
-  return(settingAGC);
+  return(setting_agc);
 }
 
-//------------------------------------------
-
-
-float peakLevel;
-uint32_t peakFreq;
-int peakIndex;
-float temppeakLevel;
-int temppeakIndex;
-
-#define BARSTART  24
-
-
-float rbw = 0;
-float vbw = 0;
-int vbwSteps = 1;
-
-#if 0
-int inData = 0;
-unsigned long  startFreq = 250000000;
-unsigned long  stopFreq = 300000000;
-unsigned long  lastFreq[6] = { 300000000, 300000000,0,0,0,0};
-int lastParameter[10];
-int parameter;
-unsigned long reg = 0;
-long offset=0;
-long offset2=0;
-static unsigned int spacing = 10000;
-double delta=0.0;
-int phase=0;
-int deltaPhase;
-int delaytime = 50;
-#endif
-
-
-#if 0
-void displayHisto ()
+void SetRefpos(int level)
 {
-  //  clearDisplay();
-  //int settingMax = 0;
-  //int settingMin = -120;
-
-  if (old_settingMax != settingMax || old_settingMin != settingMin) {
-    // Display levels at left of screen
-    tft.fillRect(0, 0, oX-2, tft.height(), DISPLAY_BLACK);
-    textWhite();
-    tft.setCursor(0,oY);             // Start at top-left corner
-    tft.println(settingMax);
-    tft.setCursor(0,tft.height() - 16);
-    tft.println(settingMin);
-    //  tft.setCursor(0,tft.height()/2);
-    //  tft.println("dB");
-    old_settingMax = settingMax;
-    old_settingMin = settingMin;
-  }
-
-  if (old_startFreq != startFreq || old_stopFreq != stopFreq) {
-    // Dsiplay frequencies
-    // Bottom of screen
-    tft.fillRect(0, tft.height()-8, tft.width(), tft.height()-1, DISPLAY_BLACK);
-    tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-    tft.setCursor(oX+2,tft.height()-8);             // Start at top-left corner
-    double f = (((double)(startFreq - lastFreq[0]))/ 1000000.0);
-    tft.print(f);
-    tft.print("MHz");
-    tft.setCursor(tft.width() - 58,tft.height()-8);
-    f = (((double)(stopFreq - lastFreq[0]))/ 1000000.0);
-    tft.print(f);
-    tft.print("MHz");
-
-    tft.setCursor(tft.width()/2 - 80 + oX,tft.height()-8);
-    tft.print("center:");
-    f = (double)((stopFreq/2 + startFreq/2 - lastFreq[0]) / 1000000.0);
-    tft.print(f);
-    tft.print("MHz");
-    old_startFreq = startFreq;
-    old_stopFreq = stopFreq;
-  }
-
-  // Top of screen
-
-  if (old_settingAttenuate != settingAttenuate || old_settingPowerGrid != settingPowerGrid) {
-    tft.fillRect(0, 0, 8*6, oY-2, DISPLAY_BLACK);
-    tft.setCursor(0,0);             // Start at top-left corner
-    tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-    tft.print("Atten:");
-    tft.print(settingAttenuate);
-    tft.setCursor(0,8);             // Start at top-left corner
-    tft.print(settingPowerGrid);
-    tft.print("dB/");
-    old_settingAttenuate = settingAttenuate;
-    old_settingPowerGrid = settingPowerGrid;
-    old_rbw = -1;
-  }  
-
-  if (old_rbw != rbw || old_vbw != vbw) {
-    tft.fillRect(56, 0, 99, oY-2, DISPLAY_BLACK);
-    tft.setCursor(56,0);             // Start at top-left corner
-    tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-    tft.print("RBW:");
-    tft.print(rbw);
-    tft.print("kHz");
-    tft.setCursor(56,8);             // Start at top-left corner
-    tft.print("VBW:");
-    tft.print(vbw);
-    tft.print("kHz");
-    old_rbw = rbw;
-    old_vbw = vbw;
-  }  
-
-  if (peakLevel > -150) {
-    tft.fillRect(oX+100, 0, 100, 8-1, DISPLAY_BLACK);
-    tft.setCursor(oX + 100,0);             // Start at top-left corner
-    tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-    tft.print("Max=");
-    tft.print((int)((peakLevel/ 2.0  - settingAttenuate) - 120.0)+settingLevelOffset);
-    tft.print("dB, ");
-    tft.print(peakFreq/ 1000000.0);
-    tft.print("MHz");
-  }
-
-  if (old_settingAverage != settingAverage || abs(old_settingSpur) != abs(settingSpur)) {
-    int x =  tft.width() - 60;
-    tft.fillRect( x, 0, 60, oY-2, DISPLAY_BLACK);
-    tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-    if (settingAverage) {
-      tft.setCursor( x,0);             // Start at top-left corner
-      tft.print("AVR:");
-      tft.print(averageText[settingAverage]);
-    }
-    if (settingSpur) {
-      tft.setCursor(x,8);             // Start at top-left corner
-      tft.print("SPUR:");
-      tft.print("ON");
-    }
-    old_settingAverage = settingAverage;
-    old_settingSpur = settingSpur;
-  }  
-
-
-
-  /*
-  for (int i=0; i<DISPLAY_POINTS - 1; i++) {
-    int delta=settingMax - settingMin;
-    DrawCheckerBoard(i);
-    double f = ((actual_t[i] / 2.0  - settingAttenuate) - 120.0) + settingLevelOffset;
-    f = (f - settingMin) * Y_GRID * dY / delta;
-    if (f >= Y_GRID * dY) f = Y_GRID * dY-1;
-    if (f < 0) f = 0;
-    double f2 = ((actual_t[i+1] / 2.0  - settingAttenuate) - 120.0) + settingLevelOffset;
-    f2 = (f2 - settingMin) * Y_GRID * dY / delta;
-    if (f2 >= Y_GRID * dY) f2 = Y_GRID * dY-1;
-    if (f2 < 0) f2 = 0;
-  int x = i;
-  int Y1 = Y_GRID * dY - 1 - (int)f;
-  int Y2 = Y_GRID * dY - 1 - (int)f2;
-  tft.drawLine(x+oX, oY+Y1, x+oX+1, oY+Y2, DISPLAY_YELLOW);
-//  tft.drawLine(x+oX, oY+Y1+1, x+oX+1, oY+Y2, DISPLAY_YELLOW);
-  }
-
+  set_trace_refpos(0, NGRIDY - level / get_trace_scale(0));
+  set_trace_refpos(1, NGRIDY - level / get_trace_scale(0));
+  set_trace_refpos(2, NGRIDY - level / get_trace_scale(0));
+  dirty = true;
+}
 
-   */
-  sendDisplay();
+void SetScale(int s) {
+  set_trace_scale(0, s);
+  set_trace_scale(1, s);
+  set_trace_scale(2, s);
 }
 
-void DisplayPoint(unsigned char *data, int i, int color)
+void SetMode(int m)
 {
-  if (i == 0)
+  if (setting_mode == m)
     return;
-  int x = i-1;
-  int delta=settingMax - settingMin;
-  double f = ((data[x] / 2.0  - settingAttenuate) - 120.0) + settingLevelOffset;
-  f = (f - settingMin) * Y_GRID * dY / delta;
-  if (f >= Y_GRID * dY) f = Y_GRID * dY-1;
-  if (f < 0) f = 0;
-  double f2 = ((data[x+1] / 2.0  - settingAttenuate) - 120.0) + settingLevelOffset;
-  f2 = (f2 - settingMin) * Y_GRID * dY / delta;
-  if (f2 >= Y_GRID * dY) f2 = Y_GRID * dY-1;
-  if (f2 < 0) f2 = 0;
-  int Y1 = Y_GRID * dY - 1 - (int)f;
-  int Y2 = Y_GRID * dY - 1 - (int)f2;
-  DrawDirty(x,min(Y2,Y1));
-  DrawDirty(x+1,min(Y2,Y1));
-  tft.drawLine(x+oX, oY+Y1, x+oX+1, oY+Y2, color);
-  //  tft.drawLine(x+oX, oY+Y1+1, x+oX+1, oY+Y2, DISPLAY_YELLOW);
-  sendDisplay();
+  reset_settings(m);
 }
 
-void DisplayPeakData(void)
+void apply_settings(void)
 {
-  double f = ((((float)actual_t[peakIndex]) / 2.0  - settingAttenuate) - 120.0) + settingLevelOffset;
-  int delta=settingMax - settingMin;
-  f = (f - settingMin) * Y_GRID * dY / delta;
-  if (f >= Y_GRID * dY) f = Y_GRID * dY-1;
-  if (f < 0) f = 0;
-  int Y1 = Y_GRID * dY - 1 - (int)f;
-  tft.setCursor(oX+peakIndex+5,oY+Y1);             // Start at top-left corner
-  tft.setTextColor(DISPLAY_WHITE);        // Draw white text
-  tft.print(peakFreq/ 1000000.0);
-  tft.setCursor(oX+peakIndex+5,oY+Y1+8);             // Start at top-left corner
-  tft.print((int)((peakLevel/ 2.0  - settingAttenuate) - 120.0)+settingLevelOffset);
-  tft.print("dB");
-  for (int x=peakIndex+5;x<peakIndex+5+6*8;x++)
-    DrawDirty(x,Y1);
+  if (setting_step_delay == 0){
+    if (MODE_LOW(setting_mode)) {
+      if      (actual_rbw >300.0)    actualStepDelay =  400;
+      else if (actual_rbw >100.0)    actualStepDelay =  500;
+      else if (actual_rbw > 30.0)    actualStepDelay =  900;
+      else if (actual_rbw > 10.0)    actualStepDelay =  900;
+      else if (actual_rbw >  3.0)    actualStepDelay = 1000;
+      else                    actualStepDelay = 1500;
+    } else {
+      if      (actual_rbw >300.0)    actualStepDelay =  900;
+      else if (actual_rbw >100.0)    actualStepDelay =  900;
+      else if (actual_rbw > 30.0)    actualStepDelay =  900;
+      else if (actual_rbw > 10.0)    actualStepDelay = 1800;
+      else if (actual_rbw >  3.0)    actualStepDelay = 6000;
+      else                    actualStepDelay = 8000;
+    }
+  } else
+    actualStepDelay = setting_step_delay;
+  PE4302_Write_Byte(setting_attenuate * 2);
+  if (setting_modulation == MO_NFM ) {
+    SI4432_Sel = 1;
+    SI4432_Write_Byte(0x7A, 1);  // Use frequency hopping channel width for FM modulation
+  } else if (setting_modulation == MO_WFM ) {
+    SI4432_Sel = 1;
+    SI4432_Write_Byte(0x7A, 10);  // Use frequency hopping channel width for FM modulation
+  } else {
+    SI4432_Sel = 1;
+    SI4432_Write_Byte(0x79, 0);  // IF no FM back to channel 0
+  }
+  SetRX(setting_mode);
+  SI4432_SetReference(setting_refer);
+  update_rbw();
 }
 
-#endif
+//------------------------------------------
+
+
+float peakLevel;
+float min_level;
+uint32_t peakFreq;
+int peakIndex;
+float temppeakLevel;
+int temppeakIndex;
 
 void setupSA(void)
 {
@@ -465,22 +356,14 @@ void setupSA(void)
   PE4302_Write_Byte(0);
 }
 
+static unsigned long old_freq[2] = { 0, 0 };
 
 void setFreq(int V, unsigned long freq)
 {
-  if (V>=0) {
-    SI4432_Sel = V;
-#ifdef USE_SI4463
-    if (SI4432_Sel == 2) {
-      freq = freq - 433000000;
-      freq = freq / 10000;  //convert to 10kHz channel starting with 433MHz
-      //      Serial.print("Set frequency Si4463 = ");
-      //      Serial.println(freq);
-      Si446x_RX ((uint8_t)freq);
-    }
-    else
-#endif
-      SI4432_Set_Frequency(freq);
+  SI4432_Sel = V;
+  if (old_freq[V] != freq) {
+    SI4432_Set_Frequency(freq);
+    old_freq[V] = freq;
   }
 }
 
@@ -496,8 +379,8 @@ void SetSwitchReceive(void) {
 
 void SetAGCLNA(void) {
   unsigned char v = 0x40;
-  if (settingAGC) v |= 0x20;
-  if (settingLNA) v |= 0x10;
+  if (setting_agc) v |= 0x20;
+  if (setting_lna) v |= 0x10;
   SI4432_Write_Byte(0x69, v);
 }
 
@@ -513,8 +396,8 @@ case M_LOW:     // Mixed into 0
     SI4432_Sel = 1;
     SetSwitchReceive();
 //    SI4432_Receive(); For noise testing only
-    SI4432_Transmit(settingDrive);
-    // SI4432_SetReference(settingRefer);
+    SI4432_Transmit(setting_drive);
+    // SI4432_SetReference(setting_refer);
     break;
 case M_HIGH:    // Direct into 1
     // SI4432_SetReference(-1); // Stop reference output
@@ -531,11 +414,11 @@ case M_HIGH:    // Direct into 1
 case M_GENLOW:  // Mixed output from 0
     SI4432_Sel = 0;
     SetSwitchTransmit();
-    SI4432_Transmit(settingDrive);
+    SI4432_Transmit(setting_drive);
 
     SI4432_Sel = 1;
     SetSwitchReceive();
-    SI4432_Transmit(settingDrive);
+    SI4432_Transmit(setting_drive);
 
     break;
 case M_GENHIGH: // Direct output from 1
@@ -545,118 +428,144 @@ case M_GENHIGH: // Direct output from 1
 
     SI4432_Sel = 1;
     SetSwitchTransmit();
-    SI4432_Transmit(settingDrive);
+    SI4432_Transmit(setting_drive);
 
     break;
   }
 }
 
-void update_rbw(uint32_t delta_f)
+void update_rbw(void)
 {
-  vbw = (delta_f)/1000.0;
-  rbw = settingBandwidth;
-//  float old_rbw = rbw;
-  if (rbw == 0)
-    rbw = 2*vbw;
-  if (rbw < 2.6)
-    rbw = 2.6;
-  if (rbw > 600)
-    rbw = 600;
-  SI4432_Sel = (settingMode & 1);
-  rbw = SI4432_SET_RBW(rbw);
-  vbwSteps = ((int)(2 * vbw / rbw));
+  setting_vbw = (setting_frequency_step)/1000.0;
+  actual_rbw = setting_rbw;
+//  float old_rbw = actual_rbw;
+  if (actual_rbw == 0)
+    actual_rbw = 2*setting_vbw;
+  if (actual_rbw < 2.6)
+    actual_rbw = 2.6;
+  if (actual_rbw > 600)
+    actual_rbw = 600;
+
+  SI4432_Sel =  MODE_SELECT(setting_mode);
+  actual_rbw = SI4432_SET_RBW(actual_rbw);
+
+  vbwSteps = ((int)(2 * setting_vbw / actual_rbw));
+
   if (vbwSteps < 1)
     vbwSteps = 1;
   dirty = true;
 }
 
-float perform(bool break_on_operation, int i, int32_t f, int extraV)
+//static int spur_old_stepdelay = 0;
+static const unsigned int spur_IF =            433900000;
+static const unsigned int spur_alternate_IF =  433700000;
+static const int spur_table[] =
 {
-  long local_IF = ((settingMode & 1) == 0?frequency_IF + (int)(rbw < 300.0?settingSpur * 1000 * rbw :0):0);
-  if (i == 0) {
-    if (settingSpeed == 0){
-      if (rbw < 10.0)
-        stepDelay = 2500;
-      else if (rbw <30.0)
-        stepDelay = 2000;
-      else if (rbw <100.0)
-        stepDelay = 1000;
-      else
-        stepDelay = 500;
-    } else
-      stepDelay = settingSpeed;
+   470000,
+   780000,
+   830000,
+   880000,
+   949000,
+  1390000,
+  1468000,
+  1830000,
+  1900000,
+  2770000,
+  2840000,
+  2880000,
+  4710000,
+  4780000,
+  4800000,
+  4880000,
+  6510000,
+  6750000,
+  6790000,
+  6860000,
+  7340000,
+  8100000,
+  8200000,
+  8880000,
+//  9970000,    10MHz!!!!!!
+ 10870000,
+ 11420000,
+ 14880000,
+ 16820000,
+};
 
-//    setupSA();
+int avoid_spur(int f)
+{
+  int window = ((int)actual_rbw ) * 1000*2;
+  if (window < 50000)
+    window = 50000;
+  if (! setting_mode == M_LOW || frequency_IF != spur_IF || actual_rbw > 300.0)
+    return(false);
+  for (unsigned int i = 0; i < (sizeof spur_table)/sizeof(int); i++) {
+    if (f/window == spur_table[i]/window) {
+//      spur_old_stepdelay = actualStepDelay;
+//      actualStepDelay += 4000;
+      return true;
+    }
+  }
+  return false;
+}
 
-    int p = settingAttenuate * 2;
-    PE4302_Write_Byte(p);
-    SetRX(settingMode);
-    SI4432_SetReference(settingRefer);
-    temppeakLevel = -150;
+static int modulation_counter = 0;
+
+char age[POINTS_COUNT];
+
+float perform(bool break_on_operation, int i, int32_t f, int tracking)
+{
+//  long local_IF = (MODE_LOW(setting_mode)?frequency_IF + (int)(actual_rbw < 300.0?setting_spur * 1000 * actual_rbw :0):0);
+  long local_IF;
+  if (MODE_HIGH(setting_mode))
+    local_IF = 0;
+  else if (avoid_spur(f))
+    local_IF = spur_alternate_IF;
+  else
+    local_IF = frequency_IF;
+
+  if (i == 0 && dirty) {
+    apply_settings();
+    scandirty = true;
+    dirty = false;
+  }
+  if (local_IF) {
     setFreq (0, local_IF);
-    if (dirty) {
-      scandirty = true;
-      dirty = false;
-    }
   }
-  volatile int subSteps = ((int)(2 * vbw / rbw));
+  if (setting_modulation == MO_AM) {
+    int p = setting_attenuate * 2 + modulation_counter;
+    PE4302_Write_Byte(p);
+    if (modulation_counter == 3)
+      modulation_counter = 0;
+    else
+      modulation_counter++;
+    chThdSleepMicroseconds(250);
+  } else if (setting_modulation == MO_NFM || setting_modulation == MO_WFM ) {
+      SI4432_Sel = 1;
+      SI4432_Write_Byte(0x79, modulation_counter);  // Use frequency hopping channel for FM modulation
+      if (modulation_counter == 3)
+        modulation_counter = 0;
+      else
+        modulation_counter++;
+      chThdSleepMicroseconds(250);
+  }
   float RSSI = -150.0;
   int t = 0;
   do {
-    int lf = (uint32_t)(f + (int)(t * 500 * rbw));
-    if (extraV)
-      setFreq (0, local_IF + lf - refferFreq[settingRefer]);    // Offset so fundamental of reffer is visible
+    int lf = (uint32_t)(f + (int)(t * 500 * actual_rbw));
+    if (tracking)
+      setFreq (0, local_IF + lf - reffer_freq[setting_refer]);    // Offset so fundamental of reffer is visible
     setFreq (1, local_IF + lf);
-    float subRSSI = SI4432_RSSI(lf, (settingMode & 1))+settingLevelOffset()+settingAttenuate;
+    if (MODE_OUTPUT(setting_mode))
+      return(0);
+    float subRSSI = SI4432_RSSI(lf, MODE_SELECT(setting_mode))+settingLevelOffset()+setting_attenuate;
     if (RSSI < subRSSI)
       RSSI = subRSSI;
     t++;
-    if (operation_requested && break_on_operation)
-      subSteps = 0;         // abort
-  } while (subSteps-- > 0);
+    if ((operation_requested && break_on_operation ) || (MODE_OUTPUT(setting_mode))) // output modes do not step.
+      break;         // abort
+  } while (t < vbwSteps);
   return(RSSI);
-#if 0
-  temp_t[i] = RSSI;
-  if (settingSubtractStorage) {
-    RSSI = RSSI - stored_t[i] ;
-  }
-  if (scandirty || settingAverage == AV_OFF)
-    actual_t[i] = RSSI;
-  else {
-    switch(settingAverage) {
-    case AV_MIN: if (actual_t[i] > RSSI) actual_t[i] = RSSI; break;
-    case AV_MAX: if (actual_t[i] < RSSI) actual_t[i] = RSSI; break;
-    case AV_2: actual_t[i] = (actual_t[i] + RSSI) / 2.0; break;
-    case AV_4: actual_t[i] = (actual_t[i]*3 + RSSI) / 4.0; break;
-    case AV_8: actual_t[i] = (actual_t[i]*7 + RSSI) / 8.0; break;
-    }
-  }
-  if (frequencies[i] > 1000000) {
-    if (temppeakLevel < actual_t[i]) {
-      temppeakIndex = i;
-      temppeakLevel = actual_t[i];
-    }
-  }
-  if (temp_t[i] == 0) {
-    SI4432_Init();
-  }
-  if (i == POINTS_COUNT -1) {
-    if (scandirty) {
-      scandirty = false;
-    }
-    peakIndex = temppeakIndex;
-    peakLevel = actual_t[peakIndex];
-    peakFreq = frequencies[peakIndex];
-    settingSpur = -settingSpur;
-    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
 }
 
 // main loop for measurement
@@ -664,63 +573,93 @@ static bool sweep(bool break_on_operation)
 {
   float RSSI;
   palClearPad(GPIOC, GPIOC_LED);
+  temppeakLevel = -150;
+  float temp_min_level = 100;
+  //  spur_old_stepdelay = 0;
+//again:
   for (int i = 0; i < sweep_points; i++) {
-again:
-    RSSI = perform(break_on_operation, i, frequencies[i], extraVFO);
-    if (settingSpur == 1)
-      temp_t[i] = RSSI;
-    else
-    {
-      if (settingSpur == -1)
-        RSSI = ( RSSI < temp_t[i] ? RSSI : temp_t[i]);
-      temp_t[i] = RSSI;
-      if (settingSubtractStorage) {
-        RSSI = RSSI - stored_t[i] ;
-      }
-      //   stored_t[i] = (SI4432_Read_Byte(0x69) & 0x0f) * 3.0 - 90.0; // Display the AGC value in thestored trace
-      if (scandirty || settingAverage == AV_OFF)
-        actual_t[i] = RSSI;
-      else {
-        switch(settingAverage) {
-        case AV_MIN: if (actual_t[i] > RSSI) actual_t[i] = RSSI; break;
-        case AV_MAX: if (actual_t[i] < RSSI) actual_t[i] = RSSI; break;
-        case AV_2: actual_t[i] = (actual_t[i] + RSSI) / 2.0; break;
-        case AV_4: actual_t[i] = (actual_t[i]*3 + RSSI) / 4.0; break;
-        case AV_8: actual_t[i] = (actual_t[i]*7 + RSSI) / 8.0; break;
-        }
-      }
-      if (frequencies[i] > 1000000) {
-        if (temppeakLevel < actual_t[i]) {
-          temppeakIndex = i;
-          temppeakLevel = actual_t[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;
+
+//    if (setting_spur == 1) {                           // First pass
+//      temp_t[i] = RSSI;
+//      continue;                                       // Skip all other processing
+//    }
+//    if (setting_spur == -1)                            // Second pass
+//      RSSI = ( RSSI < temp_t[i] ? RSSI : temp_t[i]);  // Minimum of two passes
+    temp_t[i] = RSSI;
+    if (setting_subtract_stored) {
+      RSSI = RSSI - stored_t[i] ;
+    }
+    //   stored_t[i] = (SI4432_Read_Byte(0x69) & 0x0f) * 3.0 - 90.0; // Display the AGC value in thestored trace
+    if (scandirty || setting_average == AV_OFF) {
+      actual_t[i] = RSSI;
+      age[i] = 0;
+    } else {
+      switch(setting_average) {
+      case AV_MIN:      if (actual_t[i] > RSSI) actual_t[i] = RSSI; break;
+      case AV_MAX_HOLD: if (actual_t[i] < RSSI) actual_t[i] = RSSI; break;
+      case AV_MAX_DECAY:
+        if (actual_t[i] < RSSI) {
+          actual_t[i] = RSSI;
+          age[i] = 0;
+        } else {
+          if (age[i] > 20)
+            actual_t[i] -= 0.5;
+          else
+            age[i] += 1;
         }
+        break;
+      case AV_4:  actual_t[i] = (actual_t[i] + RSSI) / 4.0; break;
+      case AV_16: actual_t[i] = (actual_t[i]*3 + RSSI) / 16.0; break;
       }
     }
-    if (i == sweep_points -1) {
-      if (settingSpur == 1) {
-        settingSpur = -1;
-        i = 0;
-        goto again;
-      }
-      if (scandirty) {
-        scandirty = false;
+    if (frequencies[i] > 1000000) {
+      if (temppeakLevel < actual_t[i]) {
+        temppeakIndex = i;
+        temppeakLevel = actual_t[i];
       }
-      peakIndex = temppeakIndex;
-      peakLevel = actual_t[peakIndex];
-      peakFreq = frequencies[peakIndex];
-      settingSpur = -settingSpur;
-      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);
-
-
     }
-    // back to toplevel to handle ui operation
-    if (operation_requested && break_on_operation)
-      return false;
+    if (temp_min_level > actual_t[i])
+      temp_min_level = actual_t[i];
+//STOP_PROFILE
   }
+//  if (setting_spur == 1) {
+//    setting_spur = -1;
+//    goto again;
+//  } else if (setting_spur == -1)
+//    setting_spur = 1;
+
+  if (scandirty) {
+    scandirty = false;
+    draw_cal_status();
+  }
+  peakIndex = temppeakIndex;
+  peakLevel = actual_t[peakIndex];
+  peakFreq = frequencies[peakIndex];
+  min_level = temp_min_level;
+#if 0                           // Auto ref level setting
+  int scale = get_trace_scale(2);
+  int rp = (NGRIDY - get_trace_refpos(2)) * scale;
+  if (scale > 0 && peakLevel > rp && peakLevel - min_level < 8 * scale ) {
+    SetRefpos((((int)(peakLevel/scale)) + 1) * scale);
+  }
+  if (scale > 0 && min_level < rp - 9*scale && peakLevel - min_level < 8 * scale ) {
+    int new_rp = (((int)((min_level + 9*scale)/scale)) - 1) * scale;
+    if (new_rp < rp)
+      SetRefpos(new_rp);
+  }
+
+#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;
 }
@@ -777,7 +716,7 @@ void PeakSearch()
   peakIndex = temppeakIndex;
   peakLevel = actual_t[peakIndex];
   peakFreq = frequencies[peakIndex];
-  settingSpur = -settingSpur;
+  setting_spur = -setting_spur;
   int peak_marker = 0;
   markers[peak_marker].enabled = true;
   markers[peak_marker].index = peakIndex;
@@ -790,9 +729,9 @@ void PeakSearch()
 }
 #endif
 
-char *averageText[] = { "OFF", "MIN", "MAX", "2", "4", "8"};
-char *dBText[] = { "1dB/", "2dB/", "5dB/", "10dB/", "20dB/"};
-int refMHz[] = { 30, 15, 10, 4, 3, 2, 1 };
+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 };
 
 void draw_cal_status(void)
 {
@@ -805,45 +744,52 @@ void draw_cal_status(void)
 
 #define XSTEP   40
 
-//  if (!sweep_enabled)
-//    perform(true, 0, frequencies[0], false);
-
   ili9341_fill(x, y, OFFSETX, HEIGHT, 0x0000);
+
+  if (MODE_OUTPUT(setting_mode))     // No cal status during output
+    return;
+  if (current_menu_is_form() && !in_selftest)
+    return;
+
   ili9341_set_background(DEFAULT_BG_COLOR);
 
   int yMax = (NGRIDY - get_trace_refpos(0)) * get_trace_scale(0);
   plot_printf(buf, BLEN, "%ddB", yMax);
   buf[5]=0;
-  ili9341_set_foreground(DEFAULT_FG_COLOR);
+  if (level_is_calibrated())
+    color = DEFAULT_FG_COLOR;
+  else
+    color = BRIGHT_COLOR_RED;
+  ili9341_set_foreground(color);
   ili9341_drawstring(buf, x, y);
 
   y += YSTEP*2;
   plot_printf(buf, BLEN, "%ddB/",(int)get_trace_scale(0));
   ili9341_drawstring(buf, x, y);
 
-  if (settingAttenuate) {
+  if (setting_attenuate) {
     ili9341_set_foreground(BRIGHT_COLOR_GREEN);
     y += YSTEP*2;
     ili9341_drawstring("Attn:", x, y);
 
     y += YSTEP;
-    plot_printf(buf, BLEN, "-%ddB", settingAttenuate);
+    plot_printf(buf, BLEN, "-%ddB", setting_attenuate);
     buf[5]=0;
     ili9341_drawstring(buf, x, y);
   }
 
-  if (settingAverage>0) {
+  if (setting_average>0) {
     ili9341_set_foreground(BRIGHT_COLOR_BLUE);
     y += YSTEP*2;
     ili9341_drawstring("Aver:", x, y);
 
     y += YSTEP;
-    plot_printf(buf, BLEN, "%s",averageText[settingAverage]);
+    plot_printf(buf, BLEN, "%s",averageText[setting_average]);
     buf[5]=0;
     ili9341_drawstring(buf, x, y);
   }
-
-  if (settingSpur) {
+#if 0
+  if (setting_spur) {
     ili9341_set_foreground(BRIGHT_COLOR_BLUE);
     y += YSTEP*2;
     ili9341_drawstring("Spur:", x, y);
@@ -852,8 +798,9 @@ void draw_cal_status(void)
     plot_printf(buf, BLEN, "ON");
     ili9341_drawstring(buf, x, y);
   }
+#endif
 
-  if (settingBandwidth)
+  if (setting_rbw)
     color = BRIGHT_COLOR_GREEN;
   else
     color = DEFAULT_FG_COLOR;
@@ -863,7 +810,7 @@ void draw_cal_status(void)
   ili9341_drawstring("RBW:", x, y);
 
   y += YSTEP;
-  plot_printf(buf, BLEN, "%dkHz", (int)rbw);
+  plot_printf(buf, BLEN, "%dkHz", (int)actual_rbw);
   buf[5]=0;
   ili9341_drawstring(buf, x, y);
 
@@ -872,15 +819,18 @@ void draw_cal_status(void)
   ili9341_drawstring("VBW:", x, y);
 
   y += YSTEP;
-  plot_printf(buf, BLEN, "%dkHz",(int)vbw);
+  plot_printf(buf, BLEN, "%dkHz",(int)setting_vbw);
   buf[5]=0;
   ili9341_drawstring(buf, x, y);
 
+  if (dirty)
+    ili9341_set_foreground(BRIGHT_COLOR_RED);
+
   y += YSTEP*2;
   ili9341_drawstring("Scan:", x, y);
 
   y += YSTEP;
-  int32_t t = (int)((2* vbwSteps * sweep_points * ( stepDelay / 100) )) /10 * (settingSpur ? 2 : 1); // in mS
+  int32_t t = (int)((2* vbwSteps * sweep_points * ( actualStepDelay / 100) )) /10  /* * (setting_spur ? 2 : 1) */; // in mS
   if (t>1000)
     plot_printf(buf, BLEN, "%dS",(t+500)/1000);
   else
@@ -890,21 +840,33 @@ void draw_cal_status(void)
   ili9341_drawstring(buf, x, y);
 
 
-  if (settingRefer >= 0) {
+  if (setting_refer >= 0) {
     ili9341_set_foreground(BRIGHT_COLOR_RED);
     y += YSTEP*2;
     ili9341_drawstring("Ref:", x, y);
 
     y += YSTEP;
-    plot_printf(buf, BLEN, "%dMHz",refMHz[settingRefer]);
+    plot_printf(buf, BLEN, "%dMHz",reffer_freq[setting_refer]/1000000);
     buf[5]=0;
     ili9341_drawstring(buf, x, y);
   }
 
+  ili9341_set_foreground(BRIGHT_COLOR_GREEN);
+  y += YSTEP*2;
+  if (MODE_LOW(setting_mode))
+      ili9341_drawstring_7x13("M:L", x, y);
+  else
+    ili9341_drawstring_7x13("M:H", x, y);
+
+
   y = HEIGHT-7 + OFFSETY;
   plot_printf(buf, BLEN, "%ddB", (int)(yMax - get_trace_scale(0) * NGRIDY));
   buf[5]=0;
-  ili9341_set_foreground(DEFAULT_FG_COLOR);
+  if (level_is_calibrated())
+    color = DEFAULT_FG_COLOR;
+  else
+    color = BRIGHT_COLOR_RED;
+  ili9341_set_foreground(color);
   ili9341_drawstring(buf, x, y);
 
 }
@@ -912,32 +874,39 @@ void draw_cal_status(void)
 // -------------------- Self testing -------------------------------------------------
 
 enum {
-  TC_SIGNAL, TC_BELOW, TC_ABOVE, TC_FLAT
+  TC_SIGNAL, TC_BELOW, TC_ABOVE, TC_FLAT, TC_MEASURE, TC_SET, TC_END,
 };
 
 enum {
-  TP_SILENT, TP_10MHZ, TP_10MHZEXTRA, TP_30MHZ
+  TP_SILENT, TPH_SILENT, TP_10MHZ, TP_10MHZEXTRA, TP_30MHZ, TPH_30MHZ
 };
 
-#define TEST_COUNT  7
+#define TEST_COUNT  14
 
 static const struct {
   int kind;
   int setup;
-  uint32_t center;      // In MHz
-  int span;             // In MHz
+  float center;      // In MHz
+  float span;        // In MHz
   float pass;
   int width;
   float stop;
 } test_case [TEST_COUNT] =
 {// Condition   Preparation     Center  Span    Pass Width  Stop
- {TC_SIGNAL,    TP_10MHZ,       10,     7,      -30, 30,    -85 },
- {TC_SIGNAL,    TP_10MHZ,       20,     7,      -50, 30,    -90 },
- {TC_SIGNAL,    TP_10MHZ,       30,     7,      -40, 30,    -90 },
- {TC_BELOW,     TP_SILENT,      200,    100,    -80, 0,     0},
- {TC_SIGNAL,    TP_10MHZEXTRA,  10,     8,      -30, 50,    -80 },
- {TC_FLAT,      TP_10MHZEXTRA,  10,     4,      -35, 20,    -80},
- {TC_SIGNAL,    TP_30MHZ,       360,    18,     -70, 20,    -100 },
+ {TC_BELOW,     TP_SILENT,      0.001,  0.0005,  -10,0,     0},         // 1 Zero Hz leakage
+ {TC_BELOW,     TP_SILENT,      0.01,  0.01,  -40,   0,     0},         // 2 Phase noise of zero Hz
+ {TC_SIGNAL,    TP_10MHZ,       20,     7,      -40, 30,    -90 },      // 3
+ {TC_SIGNAL,    TP_10MHZ,       30,     7,      -30, 30,    -90 },      // 4
+ {TC_BELOW,     TP_SILENT,      200,    100,    -75, 0,     0},         // 5  Wide band noise floor low mode
+ {TC_BELOW,     TPH_SILENT,     600,    720,    -75, 0,     0},         // 6 Wide band noise floor high mode
+ {TC_SIGNAL,    TP_10MHZEXTRA,  10,     8,      -20, 50,    -70 },      // 7 BPF loss and stop band
+ {TC_FLAT,      TP_10MHZEXTRA,  10,     4,      -25, 20,    -70},       // 8 BPF pass band flatness
+ {TC_BELOW,     TP_30MHZ,       430,    60,     -75, 0,     -85},       // 9 LPF cutoff
+ {TC_END,       0,              0,      0,      0,   0,     0},
+ {TC_MEASURE,   TP_30MHZ,       30,     7,      -25, 30,    -85 },      // 11 Measure power level and noise
+ {TC_MEASURE,   TP_30MHZ,       270,    4,      -50, 30,    -85 },       // 13 Measure powerlevel and noise
+ {TC_MEASURE,   TPH_30MHZ,      270,    4,      -50, 30,    -85 },       // 14 Calibrate power high mode
+ {TC_END,       0,              0,      0,      0,   0,     0},
 };
 
 enum {
@@ -952,17 +921,23 @@ static const  char *(test_fail_cause [TEST_COUNT]);
 static int test_status[TEST_COUNT];
 static int show_test_info = FALSE;
 static volatile int test_wait = false;
+static float test_value;
 
 static void test_acquire(int i)
 {
   pause_sweep();
+#if 0
   if (test_case[i].center < 300)
-    settingMode = 0;
+    setting_mode = M_LOW;
   else
-    settingMode = 1;
-
-  set_sweep_frequency(ST_CENTER, (int32_t)test_case[i].center * 1000000);
-  set_sweep_frequency(ST_SPAN, (int32_t)test_case[i].span * 1000000);
+    setting_mode = M_HIGH;
+#endif
+  set_sweep_frequency(ST_CENTER, (int32_t)(test_case[i].center * 1000000));
+  set_sweep_frequency(ST_SPAN, (int32_t)(test_case[i].span * 1000000));
+  SetAverage(4);
+  sweep(false);
+  sweep(false);
+  sweep(false);
   sweep(false);
   plot_into_index(measured);
   redraw_request |= REDRAW_CELLS | REDRAW_FREQUENCY;
@@ -979,13 +954,15 @@ void cell_draw_test_info(int x0, int y0)
 //  char self_test_status_buf[35];
   if (!show_test_info)
     return;
-  for (int i = -1; i < TEST_COUNT+1; i++) {
+  int i = -2;
+  do {
+    i++;
     int xpos = 25 - x0;
     int ypos = 40+i*INFO_SPACING - y0;
     unsigned int color = RGBHEX(0xFFFFFF);
     if (i == -1) {
         plot_printf(self_test_status_buf, sizeof self_test_status_buf, "Self test status:");
-    } else if (i == TEST_COUNT) {
+    } else if (test_case[i].kind == TC_END) {
         if (test_wait)
           plot_printf(self_test_status_buf, sizeof self_test_status_buf, "Touch screen to continue");
         else
@@ -1003,7 +980,7 @@ void cell_draw_test_info(int x0, int y0)
     }
     ili9341_set_foreground(color);
     cell_drawstring(self_test_status_buf, xpos, ypos);
-  }
+  } while (test_case[i].kind != TC_END);
 }
 
 #define fabs(X) ((X)<0?-(X):(X))
@@ -1063,71 +1040,133 @@ int validate_above(void) {
 }
 
 
-void test_validate(int i)
+int test_validate(int i)
 {
-  if (test_case[i].kind == TC_SIGNAL) {         // Validate signal
+//  draw_all(TRUE);
+  int current_test_status = TS_PASS;
+  switch (test_case[i].kind) {
+  case TC_SET:
+    if (test_case[i].pass == 0) {
+      if (test_value != 0)
+        SetPowerLevel(test_value);
+    } else
+      SetPowerLevel(test_case[i].pass);
+    goto common;
+  case TC_MEASURE:
+    case TC_SIGNAL:           // Validate signal
+ common:
     if (validate_peak_within(i, 5.0))                // Validate Peak
-      test_status[i] = TS_PASS;
+      current_test_status = TS_PASS;
     else if (validate_peak_within(i, 10.0))
-      test_status[i] = TS_CRITICAL;
+      current_test_status = TS_CRITICAL;
     else
-      test_status[i] = TS_FAIL;
-    if (test_status[i] != TS_PASS)
+      current_test_status = TS_FAIL;
+    if (current_test_status != TS_PASS)
       test_fail_cause[i] = "Peak ";
-    if (test_status[i] == TS_PASS) {            // Validate noise floor
+    if (current_test_status == TS_PASS) {            // Validate noise floor
       for (int j = 0; j < POINTS_COUNT/2 - test_case[i].width; j++) {
         if (actual_t[j] > test_case[i].stop - 5)
-          test_status[i] = TS_CRITICAL;
+          current_test_status = TS_CRITICAL;
         else if (actual_t[j] > test_case[i].stop) {
-          test_status[i] = TS_FAIL;
+          current_test_status = TS_FAIL;
           break;
         }
       }
       for (int j = POINTS_COUNT/2 + test_case[i].width; j < POINTS_COUNT; j++) {
         if (actual_t[j] > test_case[i].stop - 5)
-          test_status[i] = TS_CRITICAL;
+          current_test_status = TS_CRITICAL;
         else if (actual_t[j] > test_case[i].stop) {
-          test_status[i] = TS_FAIL;
+          current_test_status = TS_FAIL;
           break;
         }
       }
-      if (test_status[i] != TS_PASS)
+      if (current_test_status != TS_PASS)
         test_fail_cause[i] = "Stopband ";
     }
-
-  } else if (test_case[i].kind == TC_ABOVE) {   // Validate signal above curve
+    if (current_test_status == TS_PASS && test_case[i].kind == TC_MEASURE)
+      test_value = peakLevel;
+    else
+      test_value = 0;           //   Not valid
+    break;
+  case TC_ABOVE:   // Validate signal above curve
     for (int j = 0; j < POINTS_COUNT; j++) {
       if (actual_t[j] < test_case[i].pass + 5)
-        test_status[i] = TS_CRITICAL;
+        current_test_status = TS_CRITICAL;
       else if (actual_t[j] < test_case[i].pass) {
-        test_status[i] = TS_FAIL;
+        current_test_status = TS_FAIL;
         break;
       }
     }
-    if (test_status[i] != TS_PASS)
+    if (current_test_status != TS_PASS)
       test_fail_cause[i] = "Above ";
-
-  } else if (test_case[i].kind == TC_BELOW) {   // Validate signal below curve
-      if (validate_peak_below(i, 10.0))
-        test_status[i] = TS_PASS;
-      else if (validate_peak_below(i, 5.0))
-        test_status[i] = TS_CRITICAL;
-      else
-        test_status[i] = TS_FAIL;
-      if (test_status[i] != TS_PASS)
+    break;
+  case TC_BELOW:   // Validate signal below curve
+      current_test_status = validate_below();
+      if (current_test_status != TS_PASS)
         test_fail_cause[i] = "Above ";
-  } else if (test_case[i].kind == TC_FLAT) {   // Validate passband flatness
-    test_status[i] = validate_flatness(i);
-    if (test_status[i] != TS_PASS)
+      break;
+  case TC_FLAT:   // Validate passband flatness
+    current_test_status = validate_flatness(i);
+    if (current_test_status != TS_PASS)
       test_fail_cause[i] = "Passband ";
+    break;
+
   }
 
   // Report status
 
-  if (test_status[i] != TS_PASS || i == TEST_COUNT - 1)
+  if (current_test_status != TS_PASS || test_case[i+1].kind == TC_END)
     test_wait = true;
+//  draw_frequencies();
+//  draw_cal_status();
   draw_all(TRUE);
   resume_sweep();
+  return current_test_status;
+}
+
+void test_prepare(int i)
+{
+  setting_tracking = false; //Default test setup
+  switch(test_case[i].setup) {                // Prepare test conditions
+  case TPH_SILENT:                             // No input signal
+    SetMode(M_HIGH);
+    goto common_silent;
+  case TP_SILENT:                             // No input signal
+    SetMode(M_LOW);
+common_silent:
+    set_refer_output(-1);
+    for (int j = 0; j < POINTS_COUNT; j++)
+      stored_t[j] = test_case[i].pass;
+    break;
+  case TP_10MHZEXTRA:                         // Swept receiver
+    SetMode(M_LOW);
+    setting_tracking = true; //Sweep BPF
+    set_refer_output(2);
+    goto common;
+  case TP_10MHZ:                              // 10MHz input
+    SetMode(M_LOW);
+    set_refer_output(2);
+ common:
+
+    for (int j = 0; j < POINTS_COUNT/2 - test_case[i].width; j++)
+      stored_t[j] = test_case[i].stop;
+    for (int j = POINTS_COUNT/2 + test_case[i].width; j < POINTS_COUNT; j++)
+      stored_t[j] = test_case[i].stop;
+    for (int j = POINTS_COUNT/2 - test_case[i].width; j < POINTS_COUNT/2 + test_case[i].width; j++)
+      stored_t[j] = test_case[i].pass;
+    break;
+  case TP_30MHZ:
+    SetMode(M_LOW);
+    set_refer_output(0);
+    goto common;
+  case TPH_30MHZ:
+    SetMode(M_HIGH);
+    set_refer_output(0);
+    goto common;
+  }
+  trace[TRACE_STORED].enabled = true;
+  SetRefpos(test_case[i].pass+10);
+  draw_cal_status();
 }
 
 extern void menu_autosettings_cb(int item);
@@ -1135,49 +1174,25 @@ extern void touch_wait_release(void);
 
 void self_test(void)
 {
+  in_selftest = true;
   menu_autosettings_cb(0);
   for (int i=0; i < TEST_COUNT; i++) {          // All test cases waiting
+    if (test_case[i].kind == TC_END)
+      break;
     test_status[i] = TS_WAITING;
     test_fail_cause[i] = "";
   }
   show_test_info = TRUE;
-  for (int i=0; i < TEST_COUNT; i++) {
-    extraVFO = false; //Default test setup
-    switch(test_case[i].setup) {                // Prepare test conditions
-    case TP_SILENT:                             // No input signal
-      set_refer_output(-1);
-      for (int j = 0; j < POINTS_COUNT; j++)
-        stored_t[j] = test_case[i].pass;
-      break;
-    case TP_10MHZEXTRA:                         // Swept receiver
-      extraVFO = true; //Sweep BPF
-      goto common;
-    case TP_10MHZ:                              // 10MHz input
-      common:
-      set_refer_output(2);
-      int j;
-      for (j = 0; j < POINTS_COUNT/2 - test_case[i].width; j++)
-        stored_t[j] = test_case[i].stop;
-      for (j = POINTS_COUNT/2 + test_case[i].width; j < POINTS_COUNT; j++)
-        stored_t[j] = test_case[i].stop;
-      for (j = POINTS_COUNT/2 - test_case[i].width; j < POINTS_COUNT/2 + test_case[i].width; j++)
-        stored_t[j] = test_case[i].pass;
-      break;
-    case TP_30MHZ:
-      set_refer_output(0);
-      goto common;
-    }
-    trace[TRACE_STORED].enabled = true;
-    set_trace_refpos(0, NGRIDY - (test_case[i].pass + 30) / get_trace_scale(0));
-    set_trace_refpos(1, NGRIDY - (test_case[i].pass + 30) / get_trace_scale(0));
-    set_trace_refpos(2, NGRIDY - (test_case[i].pass + 30) / get_trace_scale(0));
-    draw_cal_status();
+  int i=0;
+  while (test_case[i].kind != TC_END) {
+    test_prepare(i);
     test_acquire(i);                        // Acquire test
-    test_validate(i);                       // Validate test
-    chThdSleepMilliseconds(2000);
+    test_status[i] = test_validate(i);                       // Validate test
+    chThdSleepMilliseconds(1000);
     if (test_status[i] != TS_PASS) {
       touch_wait_release();
     }
+    i++;
   }
   touch_wait_release();
     //  chThdSleepMilliseconds(2000);
@@ -1187,8 +1202,73 @@ void self_test(void)
   set_trace_refpos(1, NGRIDY - (-10) / get_trace_scale(0));
   set_trace_refpos(2, NGRIDY - (-10) / get_trace_scale(0));
   set_refer_output(0);
-  settingMode = 0;
+  SetMode(M_LOW);
+  SetAverage(0);
   draw_cal_status();
+  in_selftest = false;
+  menu_autosettings_cb(0);
+}
+
+void reset_calibration(void)
+{
+  SetPowerLevel(100);
+}
 
+#define CALIBRATE_RBWS  5
+const int power_rbw [5] = { 100, 300, 30, 10, 3 };
+
+void calibrate(void)
+{
+  int local_test_status;
+  float last_peak_level;
+  in_selftest = true;
+  SetPowerLevel(100);
+  menu_autosettings_cb(0);
+  int i = 10;       // calibrate low mode power on 30 MHz;
+  for (int j= 0; j < CALIBRATE_RBWS; j++ ) {
+    SetRBW(power_rbw[j]);
+    test_prepare(i);
+    test_acquire(i);                        // Acquire test
+    local_test_status = test_validate(i);                       // Validate test
+    chThdSleepMilliseconds(1000);
+    if (local_test_status != TS_PASS) {
+ //     touch_wait_release();
+    } else
+      SetPowerLevel(-25);
+  }
+  i = 11;           // Measure 270MHz in low mode
+  SetRBW(100);
+  test_prepare(i);
+  test_acquire(i);                        // Acquire test
+  last_peak_level = peakLevel;
+  local_test_status = test_validate(i);                       // Validate test
+  chThdSleepMilliseconds(1000);
+
+  config.high_level_offset = -20;           /// Preliminary setting
+
+  i = 12;           // Calibrate 270MHz in high mode
+  for (int j = 0; j < CALIBRATE_RBWS-1; j++) {
+    SetRBW(power_rbw[j]);
+    test_prepare(i);
+    test_acquire(i);                        // Acquire test
+    local_test_status = test_validate(i);                       // Validate test
+    chThdSleepMilliseconds(1000);
+    if (local_test_status != TS_PASS) {
+      touch_wait_release();
+    } else
+      SetPowerLevel(last_peak_level);
+    }
+  touch_wait_release();
+  trace[TRACE_STORED].enabled = false;
+  set_trace_refpos(0, NGRIDY - (-10) / get_trace_scale(0));
+  set_trace_refpos(1, NGRIDY - (-10) / get_trace_scale(0));
+  set_trace_refpos(2, NGRIDY - (-10) / get_trace_scale(0));
+  set_refer_output(0);
+  SetMode(M_LOW);
+  SetAverage(0);
+  draw_cal_status();
+  in_selftest = false;
   menu_autosettings_cb(0);
 }
+
+
diff --git a/si4432.c b/si4432.c
index 9cff2fa..92fb182 100644
--- a/si4432.c
+++ b/si4432.c
@@ -136,7 +136,7 @@ void SI4432_Reset(void)
   int count = 0;
   // always perform a system reset (don't send 0x87)
   SI4432_Write_Byte( 0x07, 0x80);
-  chThdSleepMilliseconds(25);
+  chThdSleepMilliseconds(50);
   // wait for chiprdy bit
   while (count++ < 100 && ( SI4432_Read_Byte ( 0x04 ) & 0x02 ) == 0) {
     chThdSleepMilliseconds(10);
@@ -149,11 +149,11 @@ void SI4432_Transmit(int d)
   SI4432_Write_Byte(0x6D, (byte) (0x1C+d));
   if (( SI4432_Read_Byte ( 0x02 ) & 0x03 ) == 2)
     return; // Already in transmit mode
-  chThdSleepMilliseconds(20);
+  chThdSleepMilliseconds(10);
   SI4432_Write_Byte( 0x07, 0x0b);
   chThdSleepMilliseconds(20);
   while (count++ < 100 && ( SI4432_Read_Byte ( 0x02 ) & 0x03 ) != 2) {
-    chThdSleepMilliseconds(1);
+    chThdSleepMilliseconds(10);
   }
 }
 
@@ -162,8 +162,9 @@ void SI4432_Receive(void)
   int count = 0;
   if (( SI4432_Read_Byte ( 0x02 ) & 0x03 ) == 1)
     return; // Already in receive mode
-  SI4432_Write_Byte( 0x07, 0x07);
   chThdSleepMilliseconds(10);
+  SI4432_Write_Byte( 0x07, 0x07);
+  chThdSleepMilliseconds(20);
   while (count++ < 100 && ( SI4432_Read_Byte ( 0x02 ) & 0x03 ) != 1) {
     chThdSleepMilliseconds(5);
   }
@@ -174,7 +175,7 @@ void SI4432_Receive(void)
 // User asks for an RBW of WISH, go through table finding the last triple
 // for which WISH is greater than the first entry, use those values,
 // Return the first entry of the following triple for the RBW actually achieved
-static short RBW_choices[] = {     // Each triple is:  ndec, fils, WISH*10
+static const short RBW_choices[] = {     // Each triple is:  ndec, fils, WISH*10
      0, 5,1,26, 5,2,28, 5,3,31, 5,4,32, 5,5,37, 5,6,42, 5,7,
     45,4,1,    49,4,2,    54,4,3,    59,4,4,    61,4,5,    72,4,6,    82,4,7,
     88,3,1,    95,3,2,   106,3,3,   115,3,4,   121,3,5,   142,3,6,   162,3,7,
@@ -217,7 +218,7 @@ void SI4432_Set_Frequency ( long Freq ) {
   int N = Freq / 10000000;
   Carrier = ( 4 * ( Freq - N * 10000000 )) / 625;
   int Freq_Band = ( N - 24 ) | ( hbsel << 5 ) | ( sbsel << 6 );
-#if 1
+#if 0
   SI4432_Write_Byte ( 0x75, Freq_Band );
   SI4432_Write_Byte ( 0x76, (Carrier>>8) & 0xFF );
   SI4432_Write_Byte ( 0x77, Carrier & 0xFF  );
@@ -226,28 +227,31 @@ void SI4432_Set_Frequency ( long Freq ) {
 #endif
 }
 
-int stepDelay = 1500;
-int settingSpeed = 0;
+int actualStepDelay = 1500;
+
 
 float SI4432_RSSI(uint32_t i, int s)
 {
   (void) i;
   int RSSI_RAW;
+  (void) i;
   // SEE DATASHEET PAGE 61
 #ifdef USE_SI4463
   if (SI4432_Sel == 2) {
     RSSI_RAW = Si446x_getRSSI();
   } else
 #endif
+//START_PROFILE
     SI4432_Sel = s;
-    chThdSleepMicroseconds(stepDelay);
+    chThdSleepMicroseconds(actualStepDelay);
     RSSI_RAW = (unsigned char)SI4432_Read_Byte( 0x26 ) ;
-    if (settingMode < 2 && RSSI_RAW == 0)
-      SI4432_Init();
-  float dBm = 0.5 * RSSI_RAW - 120.0 ;
+ //   if (MODE_INPUT(setting_mode) && RSSI_RAW == 0)
+ //     SI4432_Init();
+  float dBm = (RSSI_RAW-240)/2.0;
 #ifdef __SIMULATION__
   dBm = Simulated_SI4432_RSSI(i,s);
 #endif
+//STOP_PROFILE
   // Serial.println(dBm,2);
   return dBm ;
 }
@@ -267,7 +271,7 @@ void SI4432_Sub_Init(void)
 //  // Register 0x75 Frequency Band Select
 //  byte sbsel = 1 ;  // recommended setting
 //  byte hbsel = 0 ;  // low bands
-//  byte fb = 19 ;    // 430–439.9 MHz
+//  byte fb = 19 ;    // 430�439.9 MHz
 //  byte FBS = (sbsel << 6 ) | (hbsel << 5 ) | fb ;
 //  SI4432_Write_Byte(0x75, FBS) ;
   SI4432_Write_Byte(0x75, 0x46) ;
@@ -372,3 +376,60 @@ void PE4302_Write_Byte(unsigned char DATA )
 }
 
 #endif
+
+
+
+#if 0
+//-----------------SI4432 dummy------------------
+void SI4432_Write_Byte(unsigned char ADR, unsigned char DATA ) {}
+unsigned char SI4432_Read_Byte(unsigned char ADR) {return ADR;}
+float SI4432_SET_RBW(float WISH) {return (WISH > 600.0?600: (WISH<3.0?3:WISH));}
+void SI4432_SetReference(int p) {}
+void SI4432_Set_Frequency(long f) {}
+void PE4302_Write_Byte(unsigned char DATA ) {}
+void PE4302_init(void) {}
+#endif
+
+#ifdef __SIMULATION__
+unsigned long seed = 123456789;
+extern float actual_rbw;
+float myfrand(void)
+{
+  seed = (unsigned int) (1103515245 * seed + 12345) ;
+  return ((float) seed) / 1000000000.0;
+}
+#define NOISE  ((myfrand()-2) * 2)  // +/- 4 dBm noise
+extern int settingAttenuate;
+
+//#define LEVEL(i, f, v) (v * (1-(fabs(f - frequencies[i])/actual_rbw/1000)))
+
+float LEVEL(uint32_t i, uint32_t f, int v)
+{
+  float dv;
+  float df = fabs((float)f - (float)i);
+  if (df < actual_rbw*1000)
+    dv = df/(actual_rbw*1000);
+  else
+    dv =  1 + 50*(df - actual_rbw*1000)/(actual_rbw*1000);
+  return (v - dv - settingAttenuate);
+}
+
+float Simulated_SI4432_RSSI(uint32_t i, int s)
+{
+  SI4432_Sel = s;
+  float v = -100 + log10(actual_rbw)*10 + NOISE;
+  if(s == 0) {
+    v = fmax(LEVEL(i,10000000,-20),v);
+    v = fmax(LEVEL(i,20000000,-40),v);
+    v = fmax(LEVEL(i,30000000,-30),v);
+    v = fmax(LEVEL(i,40000000,-90),v);
+  } else {
+    v = fmax(LEVEL(i,320000000,-20),v);
+    v = fmax(LEVEL(i,340000000,-40),v);
+    v = fmax(LEVEL(i,360000000,-30),v);
+    v = fmax(LEVEL(i,380000000,-90),v);
+  }
+  return(v);
+}
+
+#endif
diff --git a/ui.c b/ui.c
index 5460ebe..544d47b 100644
--- a/ui.c
+++ b/ui.c
@@ -1,5 +1,4 @@
-/*
- * Copyright (c) 2014-2015, TAKAHASHI Tomohiro (TTRFTECH) edy555@gmail.com
+/* Copyright (c) 2014-2015, TAKAHASHI Tomohiro (TTRFTECH) edy555@gmail.com
  * All rights reserved.
  *
  * This is free software; you can redistribute it and/or modify
@@ -31,6 +30,7 @@ uistat_t uistat = {
  lever_mode: LM_MARKER,
  marker_delta: FALSE,
  marker_tracking : FALSE,
+ text : "",
 };
 
 #define NO_EVENT                    0
@@ -433,8 +433,11 @@ enum {
   MT_SUBMENU,
   MT_CALLBACK,
   MT_CANCEL,
+  MT_TITLE,
   MT_CLOSE
 };
+#define MT_FORM 0x80        // Or with menu type to get large button with current value
+#define MT_MASK(x) (0x7F & (x))
 
 typedef void (*menuaction_cb_t)(int item, uint8_t data);
 
@@ -634,6 +637,14 @@ menu_transform_filter_cb(int item, uint8_t data)
   domain_mode = (domain_mode & ~TD_FUNC) | data;
   ui_mode_normal();
 }
+
+static void
+menu_bandwidth_cb(int item)
+{
+  bandwidth = item;
+  draw_menu();
+}
+
 static void 
 choose_active_marker(void)
 {
@@ -937,12 +948,23 @@ const menuitem_t menu_transform[] = {
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
 
+const menuitem_t menu_bandwidth[] = {
+  { MT_CALLBACK, 0, "1 kHz", menu_bandwidth_cb },
+  { MT_CALLBACK, 0, "300 Hz", menu_bandwidth_cb },
+  { MT_CALLBACK, 0, "100 Hz", menu_bandwidth_cb },
+  { MT_CALLBACK, 0, "30 Hz", menu_bandwidth_cb },
+  { MT_CALLBACK, 0, "10 Hz", menu_bandwidth_cb },
+  { MT_CANCEL, 0, S_LARROW" BACK", NULL },
+  { MT_NONE, 0, NULL, NULL } // sentinel
+};
+
 const menuitem_t menu_display[] = {
   { MT_SUBMENU, 0, "TRACE", menu_trace },
   { MT_SUBMENU, 0, "FORMAT", menu_format },
   { MT_SUBMENU, 0, "SCALE", menu_scale },
   { MT_SUBMENU, 0, "CHANNEL", menu_channel },
   { MT_SUBMENU, 0, "TRANSFORM", menu_transform },
+  { MT_SUBMENU, 0, "BANDWIDTH", menu_bandwidth },  
   { MT_CANCEL, 0, S_LARROW" BACK", NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
@@ -1046,11 +1068,19 @@ const menuitem_t menu_top[] = {
 };
 #endif
 
+
+#define MENU_BUTTON_WIDTH  60
+#define MENU_BUTTON_START  (320-MENU_BUTTON_WIDTH)
+#define MENU_FORM_WIDTH    295
+#define MENU_FORM_START    (320 - MENU_FORM_WIDTH)
+#define MENU_BUTTON_HEIGHT 30
+#define NUM_INPUT_HEIGHT   30
+
 #include "ui_sa.c"
 
 #define MENU_STACK_DEPTH_MAX 4
 const menuitem_t *menu_stack[MENU_STACK_DEPTH_MAX] = {
-  menu_top, NULL, NULL, NULL
+  menu_mode, NULL, NULL, NULL
 };
 
 static void
@@ -1058,7 +1088,7 @@ ensure_selection(void)
 {
   const menuitem_t *menu = menu_stack[menu_current_level];
   int i;
-  for (i = 0; menu[i].type != MT_NONE; i++)
+  for (i = 0; MT_MASK(menu[i].type) != MT_NONE; i++)
     ;
   if (selection >= i)
     selection = i-1;
@@ -1069,23 +1099,36 @@ menu_move_back(void)
 {
   if (menu_current_level == 0)
     return;
+  erase_menu_buttons();
   menu_current_level--;
   ensure_selection();
-  erase_menu_buttons();
   draw_menu();
 }
 
 static void
 menu_push_submenu(const menuitem_t *submenu)
 {
+  erase_menu_buttons();
   if (menu_current_level < MENU_STACK_DEPTH_MAX-1)
     menu_current_level++;
   menu_stack[menu_current_level] = submenu;
   ensure_selection();
-  erase_menu_buttons();
+  if (menu_is_form(submenu)) {
+    redraw_frame();
+    area_width = 0;
+  } else {
+    redraw_frame();
+    request_to_redraw_grid();
+    area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;
+  }
   draw_menu();
 }
 
+int current_menu_is_form(void)
+{
+  return menu_is_form(menu_stack[menu_current_level]);
+}
+
 /*
 static void
 menu_move_top(void)
@@ -1105,7 +1148,7 @@ menu_invoke(int item)
   const menuitem_t *menu = menu_stack[menu_current_level];
   menu = &menu[item];
 
-  switch (menu->type) {
+  switch (menu->type & 0x0f) {
   case MT_NONE:
   case MT_BLANK:
   case MT_CLOSE:
@@ -1130,10 +1173,6 @@ menu_invoke(int item)
   }
 }
 
-#define MENU_BUTTON_WIDTH  60
-#define MENU_BUTTON_HEIGHT 30
-#define NUM_INPUT_HEIGHT   30
-
 #define KP_WIDTH     48
 #define KP_HEIGHT    48
 // Key x, y position (0 - 15) on screen
@@ -1259,6 +1298,7 @@ draw_keypad(void)
     ili9341_set_background(bg);
     int x = KP_GET_X(keypads[i].x);
     int y = KP_GET_Y(keypads[i].y);
+//     ili9341_fill(x, y, KP_WIDTH, KP_HEIGHT, DEFAULT_MENU_TEXT_COLOR); // black area around button, causes flicker....
     ili9341_fill(x+2, y+2, KP_WIDTH-4, KP_HEIGHT-4, bg);
     ili9341_drawfont(keypads[i].c,
                      x + (KP_WIDTH - NUM_FONT_GET_WIDTH) / 2,
@@ -1379,6 +1419,11 @@ menu_item_modify_attribute(const menuitem_t *menu, int item,
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
     }
+  } else if (menu == menu_bandwidth) {
+      if (item == bandwidth) {
+        *bg = 0x0000;
+        *fg = 0xffff;
+      }
   } else if (menu == menu_transform) {
       if ((item == 0 && (domain_mode & DOMAIN_MODE) == DOMAIN_TIME)
        || (item == 1 && (domain_mode & TD_FUNC) == TD_FUNC_LOWPASS_IMPULSE)
@@ -1409,30 +1454,85 @@ static void
 draw_menu_buttons(const menuitem_t *menu)
 {
   int i = 0;
+  char text[30];
   for (i = 0; i < 7; i++) {
     const char *l1, *l2;
-    if (menu[i].type == MT_NONE)
+    if (MT_MASK(menu[i].type) == MT_NONE)
       break;
-    if (menu[i].type == MT_BLANK)
+    if (MT_MASK(menu[i].type) == MT_BLANK)
       continue;
     int y = MENU_BUTTON_HEIGHT*i;
-    uint16_t bg = config.menu_normal_color;
-    uint16_t fg = DEFAULT_MENU_TEXT_COLOR;
+    uint16_t bg;
+    uint16_t fg;
+    if (MT_MASK(menu[i].type) == MT_TITLE) {
+      fg = config.menu_normal_color;
+      bg = DEFAULT_MENU_TEXT_COLOR;
+    } else {
+      bg = config.menu_normal_color;
+      fg = DEFAULT_MENU_TEXT_COLOR;
+    }
     // focus only in MENU mode but not in KEYPAD mode
     if (ui_mode == UI_MENU && i == selection)
       bg = config.menu_active_color;
-    ili9341_fill(320-MENU_BUTTON_WIDTH, y, MENU_BUTTON_WIDTH, MENU_BUTTON_HEIGHT-2, bg);
-    
-    menu_item_modify_attribute(menu, i, &fg, &bg);
+
+    uint16_t old_bg = bg;
+    int active_button_width;
+    int active_button_start;
+    menu_item_modify_attribute(menu, i, &fg, &bg);      // before plot_printf to create status text
+    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
+        keypad_mode = menu[i].data;
+        fetch_numeric_target();
+      }
+      plot_printf(text, sizeof text, menu[i].label, uistat.text);
+    }
+    else {
+      active_button_width = MENU_BUTTON_WIDTH;
+      active_button_start = 320 - MENU_BUTTON_WIDTH;
+    }
+    ili9341_fill(active_button_start, y, active_button_width, MENU_BUTTON_HEIGHT-4, old_bg);    // Set button to unmodified background color
+#if 0
+    // 3D button accent
+    int bw = 320;
+
+    if (MT_MASK(menu[i].type) != MT_TITLE) {
+        ili9341_fill(bw-active_button_width, y,    2,                      MENU_BUTTON_HEIGHT-4, LIGHT_GREY);    // Set button to unmodified background color
+        ili9341_fill(bw-active_button_width, y,    active_button_width,    2,                      LIGHT_GREY);    // Set button to unmodified background color
+        ili9341_fill(bw-2, y,    2,                      MENU_BUTTON_HEIGHT-4, DARK_GREY);    // Set button to unmodified background color
+        ili9341_fill(bw-active_button_width, y+MENU_BUTTON_HEIGHT-4, active_button_width, 2, DARK_GREY);    // Set button to unmodified background color
+    }
+#endif
+
     ili9341_set_foreground(fg);
     ili9341_set_background(bg);
+    if (menu[i].type & MT_FORM) {
+      ili9341_fill(active_button_start+2, y+2, active_button_width-4, FONT_GET_HEIGHT*2+8, bg);
+      ili9341_drawstring_size(text, active_button_start+6, y+6, 2);
+    } else {
     if (menu_is_multiline(menu[i].label, &l1, &l2)) {
-      ili9341_fill(320-MENU_BUTTON_WIDTH+3, y+5, MENU_BUTTON_WIDTH-6, 2+FONT_GET_HEIGHT+1+FONT_GET_HEIGHT+2, bg);
-      ili9341_drawstring(l1, 320-MENU_BUTTON_WIDTH+5, y+7);
-      ili9341_drawstring(l2, 320-MENU_BUTTON_WIDTH+5, y+7+FONT_GET_HEIGHT+1);
+#define BIG_BUTTON_FONT 1
+#ifdef BIG_BUTTON_FONT
+#undef FONT_HEIGHT
+#define FONT_HEIGHT 13
+      ili9341_fill(active_button_start+1, y+1, active_button_width-2, 13+13 -2, bg);
+      ili9341_drawstring_7x13(l1, active_button_start+2, y+1);
+      ili9341_drawstring_7x13(l2, active_button_start+2, y+1+13-1);
+#else
+      ili9341_fill(active_button_start+3, y+5, active_button_width-6, 2+FONT_GET_HEIGHT+1+FONT_GET_HEIGHT+2, bg);
+      ili9341_drawstring(l1, active_button_start+5, y+7);
+      ili9341_drawstring(l2, active_button_start+5, y+7+FONT_GET_HEIGHT+1);
+#endif
     } else {
-      ili9341_fill(320-MENU_BUTTON_WIDTH+3, y+8, MENU_BUTTON_WIDTH-6, 2+FONT_GET_HEIGHT+2, bg);
-      ili9341_drawstring(menu[i].label, 320-MENU_BUTTON_WIDTH+5, y+10);
+#ifdef BIG_BUTTON_FONT
+      ili9341_fill(active_button_start+1, y+1, active_button_width-2, 13+13 -2, bg);
+      ili9341_drawstring_7x13(menu[i].label, active_button_start+2, y+6);
+#else
+      ili9341_fill(active_button_start+3, y+8, active_button_width-6, 2+FONT_GET_HEIGHT+2, bg);
+      ili9341_drawstring(menu[i].label, active_button_start+5, y+10);
+#endif
+    }
     }
   }
 }
@@ -1456,17 +1556,24 @@ menu_apply_touch(void)
 
   touch_position(&touch_x, &touch_y);
   for (i = 0; i < 7; i++) {
-    if (menu[i].type == MT_NONE)
+    if (MT_MASK(menu[i].type) == MT_NONE)
       break;
-    if (menu[i].type == MT_BLANK)
+    if (MT_MASK(menu[i].type == MT_BLANK) || MT_MASK(menu[i].type) == MT_TITLE)
       continue;
     int y = MENU_BUTTON_HEIGHT*i;
-    if (y < touch_y && touch_y < y+MENU_BUTTON_HEIGHT && 320-MENU_BUTTON_WIDTH < touch_x) {
+    int active_button_width;
+    if (menu[i].type & MT_FORM)
+      active_button_width = MENU_FORM_WIDTH;
+    else
+      active_button_width = MENU_BUTTON_WIDTH;
+
+    if (y < touch_y && touch_y < y+MENU_BUTTON_HEIGHT && 320-active_button_width < touch_x) {
       menu_select_touch(i);
       return;
     }
   }
-
+  if (menu_is_form(menu))
+    return;
   touch_wait_release();
   ui_mode_normal();
 }
@@ -1480,7 +1587,8 @@ draw_menu(void)
 static void
 erase_menu_buttons(void)
 {
-  ili9341_fill(320-MENU_BUTTON_WIDTH, 0, MENU_BUTTON_WIDTH, MENU_BUTTON_HEIGHT*7, DEFAULT_BG_COLOR);
+  ili9341_fill(area_width, 0, 320 - area_width, area_height, DEFAULT_BG_COLOR);
+//  ili9341_fill(320-MENU_BUTTON_WIDTH, 0, MENU_BUTTON_WIDTH, MENU_BUTTON_HEIGHT*7, DEFAULT_BG_COLOR);
 }
 
 static void
@@ -1643,7 +1751,8 @@ ui_mode_keypad(int _keypad_mode)
   ui_mode = UI_KEYPAD;
   area_width = AREA_WIDTH_NORMAL - MENU_BUTTON_WIDTH;
   area_height = HEIGHT - 32;
-  draw_menu();
+  if (!menu_is_form(menu_stack[menu_current_level]))
+    draw_menu();
   draw_keypad();
   draw_numeric_area_frame();
   draw_numeric_input("");
@@ -2064,8 +2173,12 @@ ui_process_keypad(void)
   }
 
   redraw_frame();
-  request_to_redraw_grid();
-  ui_mode_normal();
+  if (menu_is_form(menu_stack[menu_current_level]))
+    ui_mode_menu(); //Reactivate menu after keypad
+  else {
+    ui_mode_normal();
+    request_to_redraw_grid();
+  }
   //redraw_all();
   touch_start_watchdog();
 }
@@ -2214,6 +2327,10 @@ void
 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
+    ui_mode_menu();
+  }
   if (operation_requested&OP_LEVER || previous_button_state != button_state) {
     ui_process_lever();
     previous_button_state = button_state;
diff --git a/ui_sa.c b/ui_sa.c
index 00e5aef..d997f30 100644
--- a/ui_sa.c
+++ b/ui_sa.c
@@ -14,33 +14,48 @@ void SetAttenuation(int);
 void SetPowerLevel(int);
 void SetGenerate(int);
 void SetRBW(int);
+void SetDrive(int d);
+void SetIF(int f);
+void SetStepDelay(int t);
+extern int setting_rbw;
 void SetSpur(int);
 int GetSpur(void);
 void SetAverage(int);
 int GetAverage(void);
+extern int setting_average;
 void  SetStorage(void);
 void  SetClearStorage(void);
 void  SetSubtractStorage(void);
 void toggle_waterfall(void);
 void SetMode(int);
 int GetMode(void);
+void SetRefpos(int);
+void SetScale(int);
 void AllDirty(void);
 void MenuDirty(void);
+void ToggleLNA(void);
+void ToggleAGC(void);
 void redrawHisto(void);
 void self_test(void);
 extern int32_t frequencyExtra;
-extern int extraVFO;
-extern int settingDrive;
-extern int settingLNA;
-extern int settingAGC;
-extern int settingSpeed;
-extern int stepDelay;
+extern int setting_tracking;
+extern int setting_drive;
+extern int setting_lna;
+extern int setting_agc;
+void SetModulation(int);
+extern int setting_modulation;
+// extern int settingSpeed;
+extern int setting_step_delay;
+
+
 
 enum {
-  KM_START, KM_STOP, KM_CENTER, KM_SPAN, KM_CW, KM_REFPOS, KM_SCALE, KM_ATTENUATION, KM_ACTUALPOWER, KM_IF, KM_SAMPLETIME, KM_DRIVE
+  KM_START=1, KM_STOP, KM_CENTER, KM_SPAN, KM_CW, KM_REFPOS, KM_SCALE, KM_ATTENUATION,
+  KM_ACTUALPOWER, KM_IF, KM_SAMPLETIME, KM_DRIVE, KM_LOWOUTLEVEL, KM_HIGHOUTLEVEL
 };
 
 
+
 #define KP_X(x) (48*(x) + 2 + (320-BUTTON_WIDTH-192))
 #define KP_Y(y) (48*(y) + 2)
 
@@ -128,6 +143,7 @@ static const keypads_t keypads_level[] = {
 
 
 static const keypads_t * const keypads_mode_tbl[] = {
+  NULL,         // never used
   keypads_freq, // start
   keypads_freq, // stop
   keypads_freq, // center
@@ -140,6 +156,8 @@ static const keypads_t * const keypads_mode_tbl[] = {
   keypads_freq, // IF
   keypads_level, // sample time
   keypads_scale, // drive
+  keypads_level,    // KM_LOWOUTLEVEL
+  keypads_level,    // KM_HIGHOUTLEVEL
 };
 
 #ifdef __VNA__
@@ -149,7 +167,7 @@ static const char * const keypad_mode_label[] = {
 #endif
 #ifdef __SA__
 static const char * const keypad_mode_label[] = {
-  "START", "STOP", "CENTER", "SPAN", "CW FREQ", "REFPOS", "SCALE", "ATTENUATION", "ACTUALPOWER", "IF", "SAMPLE TIME", "DRIVE"
+  "error", "START", "STOP", "CENTER", "SPAN", "CW FREQ", "REFPOS", "SCALE", "ATTENUATION", "ACTUALPOWER", "IF", "SAMPLE TIME", "DRIVE", "LEVEL", "LEVEL"
 };
 #endif
 
@@ -159,20 +177,32 @@ static const char * const keypad_mode_label[] = {
 
 int generator_enabled = false;
 
+extern const menuitem_t  menu_lowoutputmode[];
+extern const menuitem_t  menu_highoutputmode[];
+extern const menuitem_t  menu_modulation[];
+extern const menuitem_t  menu_top[];
+extern const menuitem_t  menu_tophigh[];
+
 static void menu_mode_cb(int item, uint8_t data)
 {
   (void)data;
+  SetMode(item-1);
+//  draw_cal_status();
   switch (item) {
-  case 4: // Change reference output
+  case 1:
+    menu_push_submenu(menu_top);
     break;
-  default:
-    SetMode(item);
-    menu_move_back();
-    ui_mode_normal();
-    draw_cal_status();
+  case 2:
+    menu_push_submenu(menu_tophigh);
+    break;
+  case 3:
+    menu_push_submenu(menu_lowoutputmode);
+    break;
+  case 4:
+    menu_push_submenu(menu_highoutputmode);
     break;
   }
-
+//  draw_cal_status();
 }
 
 extern int dirty;
@@ -180,60 +210,69 @@ void menu_autosettings_cb(int item, uint8_t data)
 {
   (void)item;
   (void)data;
-  SetMode(M_LOW);
-//  set_sweep_frequency(ST_START, (int32_t) 0);
-//  set_sweep_frequency(ST_STOP, (int32_t) 300000000);
-
-  int value = 10; // 10dB/
-  set_trace_scale(0, value);
-  set_trace_scale(1, value);
-  set_trace_scale(2, value);
-
-  value = -10; // Top at -10dB
-  set_trace_refpos(0, - value / get_trace_scale(0) + NGRIDY);
-  set_trace_refpos(1, - value / get_trace_scale(0) + NGRIDY);
-  set_trace_refpos(2, - value / get_trace_scale(0) + NGRIDY);
+  int current_mode = GetMode();
+  SetMode(-1);              // Force setmode to do something
+  SetMode(current_mode);
 
   active_marker = 0;
-  menu_marker_type_cb(M_REFERENCE,M_REFERENCE);
+  for (int i = 1; i<MARKER_COUNT; i++ ) {
+    markers[i].enabled = false;
+  }
+  markers[0].enabled = true;
+  markers[0].mtype = M_REFERENCE;
+
+  //  set_refer_output(1);
 
-  set_refer_output(1);
-  SetAttenuation(0);
-  SetPowerLevel(100); // Reset
+  //  SetPowerLevel(100); // Reset
   SetClearStorage();
-  SetRBW(0);
   dirty = true;
-  menu_move_back();
+  //  menu_move_back();   // stay in input menu
   ui_mode_normal();
   draw_cal_status();
 }
 
+static void menu_calibrate_cb(int item, uint8_t data)
+{
+  (void)data;
+  switch (item) {
+  case 1:
+    calibrate();
+    menu_move_back();
+    ui_mode_normal();
+    break;
+  case 2:
+    reset_calibration();
+    draw_menu();
+    break;
+  }
+}
+
 static void menu_config_cb(int item, uint8_t data)
 {
   (void)data;
   switch (item) {
   case 0:
-      touch_cal_exec();
-      redraw_frame();
-      request_to_redraw_grid();
-      draw_menu();
-      break;
+    touch_cal_exec();
+    redraw_frame();
+    request_to_redraw_grid();
+    draw_menu();
+    break;
   case 1:
-      touch_draw_test();
-      redraw_frame();
-      request_to_redraw_grid();
-      draw_menu();
-      break;
+    touch_draw_test();
+    redraw_frame();
+    request_to_redraw_grid();
+    draw_menu();
+    break;
   case 2:
-      menu_move_back();
-      ui_mode_normal();
-      self_test();
-      break;
-  case 3:
-      show_version();
-      redraw_frame();
-      request_to_redraw_grid();
-      draw_menu();
+    menu_move_back();
+    ui_mode_normal();
+    self_test();
+    break;
+  case 4:
+    show_version();
+    redraw_frame();
+    request_to_redraw_grid();
+    draw_menu();
   }
 }
 
@@ -246,24 +285,38 @@ static void menu_dfu_cb(int item, uint8_t data)
   }
 }
 
-int menu_refer_value[]={-1,0,1,2,3,4,5,6};
-static void menu_refer_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"};
+static void menu_modulation_cb(int item, uint8_t data)
 {
   (void)data;
 //Serial.println(item);
-  set_refer_output(menu_refer_value[item]); 
+  SetModulation(menu_modulation_value[item]);
   menu_move_back();
-  ui_mode_normal();
+//  ui_mode_normal();   // Stay in menu mode
   draw_cal_status();
 }
 
-static void menu_refer_cb2(int item, uint8_t data)
+
+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;
 //Serial.println(item);
-  set_refer_output(menu_refer_value[item+5]); 
+  set_refer_output(menu_reffer_value[item]);
   menu_move_back();
-  ui_mode_normal();
+//  ui_mode_normal();   // Stay in menu mode
+  draw_cal_status();
+}
+
+static void menu_reffer_cb2(int item, uint8_t data)
+{
+  (void)data;
+//Serial.println(item);
+  set_refer_output(menu_reffer_value[item+5]);
+  menu_move_back();
+  //  ui_mode_normal();   // Stay in menu mode
   draw_cal_status();
 }
 
@@ -271,13 +324,15 @@ static void menu_spur_cb(int item, uint8_t data)
 {
   (void)data;
   (void)item;
+#if 0
   if (GetSpur())
     SetSpur(0);
   else
     SetSpur(1); // must be 0 or 1 !!!!
-  menu_move_back();
+//  menu_move_back();
   ui_mode_normal();
   draw_cal_status();
+#endif
 }
 
 static void menu_storage_cb(int item, uint8_t data)
@@ -331,7 +386,7 @@ static void menu_marker_type_cb(int item, uint8_t data)
 }
 
 
-int rbwsel[]={0,3,10,30,100,300};
+const int rbwsel[]={0,3,10,30,100,300};
 
 static void menu_rbw_cb(int item, uint8_t data)
 {
@@ -347,9 +402,7 @@ int menu_dBper_value[]={1,2,5,10,20};
 static void menu_dBper_cb(int item, uint8_t data)
 {
   (void)data;
-  set_trace_scale(0, menu_dBper_value[item]);
-  set_trace_scale(1, menu_dBper_value[item]);
-  set_trace_scale(2, menu_dBper_value[item]);
+  SetScale(menu_dBper_value[item]);
   menu_move_back();
   ui_mode_normal();
   draw_cal_status();
@@ -382,9 +435,53 @@ static void choose_active_marker(void)
 
 static void menu_scale_cb(int item, uint8_t data)
 {
-  (void)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 = KM_REFPOS + item;
+  int km = data;
+  (void) item;
+
 //  if (km == KM_SCALE && trace[uistat.current_trace].type == TRC_DELAY) {
 //    km = KM_SCALEDELAY;
 //  }
@@ -393,23 +490,25 @@ static void menu_scale_cb(int item, uint8_t data)
     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)data;
+  (void)item;
   int status;
-  int km = KM_ACTUALPOWER+item;
   status = btn_wait_release();
   if (status & EVT_BUTTON_DOWN_LONG) {
-    ui_mode_numeric(km);
+    ui_mode_numeric(data);
     //    ui_process_numeric();
   } else {
-    ui_mode_keypad(km);
+    ui_mode_keypad(data);
     ui_process_keypad();
   }
   draw_cal_status();
@@ -420,13 +519,13 @@ static void menu_settings2_cb(int item, uint8_t data)
   (void)data;
   switch(item) {
   case 0:
-    settingAGC = !settingAGC;
+    ToggleAGC();
     break;
   case 1:
-    settingLNA = !settingLNA;
+    ToggleLNA();;
     break;
   case 2:
-    extraVFO = !extraVFO;
+    toggle_tracking();
     break;
   }
   draw_cal_status();
@@ -437,22 +536,23 @@ static void menu_stimulus_cb(int item, uint8_t data)
 {
   (void) data;
   int status;
-  switch (item) {
-  case 0: /* START */
-  case 1: /* STOP */
-  case 2: /* CENTER */
-  case 3: /* SPAN */
-  case 4: /* CW */
+  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(item);
+      ui_mode_numeric(km);
 //      ui_process_numeric();
     } else {
-      ui_mode_keypad(item);
+      ui_mode_keypad(km);
       ui_process_keypad();
     }
     break;
-  case 5: /* PAUSE */
+  case KM_CW+1: /* PAUSE */
     toggle_sweep();
     menu_move_back();
     ui_mode_normal();
@@ -462,18 +562,43 @@ static void menu_stimulus_cb(int item, uint8_t data)
   draw_cal_status();
 }
 
-//static void menu_marker_sel_cb(int);
-//static void menu_marker_op_cb(int);
-
 // ===[MENU DEFINITION]=========================================================
 
+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_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_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_NONE, 0, NULL, NULL } // sentinel
+};
+
 static const menuitem_t  menu_average[] = {
-  { MT_CALLBACK, 0, "OFF",   menu_average_cb},
-  { MT_CALLBACK, 0, "MIN",   menu_average_cb},
-  { MT_CALLBACK, 0, "MAX",   menu_average_cb},
-  { MT_CALLBACK, 0, " 2 ",   menu_average_cb},
+  { 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, " 8 ",   menu_average_cb},
+  { MT_CALLBACK, 0, " 16 ",   menu_average_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE, 0, NULL, NULL } // sentinel
 };
@@ -509,36 +634,47 @@ static const menuitem_t menu_dBper[] = {
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
-static const menuitem_t menu_refer2[] = {
-  { MT_CALLBACK, 0, "3MHz" ,   menu_refer_cb2},
-  { MT_CALLBACK, 0, "2MHz" ,   menu_refer_cb2},
-  { MT_CALLBACK, 0, "1MHz" ,   menu_refer_cb2},
-  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
+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_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
-static const menuitem_t menu_refer[] = {
-  { MT_CALLBACK, 0, "OFF"  ,   menu_refer_cb},
-  { MT_CALLBACK, 0, "30MHz",   menu_refer_cb},
-  { MT_CALLBACK, 0, "15MHz",   menu_refer_cb},
-  { MT_CALLBACK, 0, "10MHz",   menu_refer_cb},
-  { MT_CALLBACK, 0, "4MHz" ,   menu_refer_cb},
-  { MT_SUBMENU,  0, S_RARROW" MORE", menu_refer2},
-  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
+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_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_scale[] = {
-  { MT_CALLBACK, 0, "\2REF\0LEVEL",  menu_scale_cb},
-  { MT_SUBMENU,  0, "\2SCALE/\0DIV",     menu_dBper},
-  { MT_CALLBACK, 0, "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_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
+};
+
+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
 };
 
+
+
 static const menuitem_t menu_stimulus[] = {
   { MT_CALLBACK, 0, "START",            menu_stimulus_cb},
   { MT_CALLBACK, 0, "STOP",             menu_stimulus_cb},
@@ -560,12 +696,24 @@ static const menuitem_t menu_marker_type[] = {
   { 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_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_SUBMENU,  0, "\2SEARCH\0MARKER",     menu_marker_search},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
@@ -582,24 +730,13 @@ static const menuitem_t menu_marker[] = {
 };
 
 static const menuitem_t menu_dfu[] = {
-  { MT_CALLBACK, 0, "ENTER DFU", menu_dfu_cb},
-  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
-  { MT_NONE,     0, NULL, NULL } // sentinel
-};
-
-static const menuitem_t menu_mode[] = {
-  { MT_CALLBACK, 0, "\2LOW\0INPUT", menu_mode_cb},
-  { MT_CALLBACK, 0, "\2HIGH\0INPUT",menu_mode_cb},
-  { MT_CALLBACK, 0, "\2LOW\0OUTPUT", menu_mode_cb},
-  { MT_CALLBACK, 0, "\2HIGH\0OUTPUT",menu_mode_cb},
-  { MT_SUBMENU,  0, "\2REFER\0OUTPUT",menu_refer},
-  { 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
 };
 
 static const menuitem_t menu_settings2[] =
 {
-//  { MT_CALLBACK, 0, "TRACK",            menu_settings2_cb},
   { MT_CALLBACK, 0, "AGC",              menu_settings2_cb},
   { MT_CALLBACK, 0, "LNA",              menu_settings2_cb},
   { MT_CALLBACK, 0, "BPF",              menu_settings2_cb},
@@ -609,37 +746,87 @@ static const menuitem_t menu_settings2[] =
 
 static const menuitem_t menu_settings[] =
 {
-  { MT_CALLBACK, 0, "\2ACTUAL\0POWER",  menu_settings_cb},
-  { MT_CALLBACK, 0, "\2IF\0FREQ",       menu_settings_cb},
-  { MT_CALLBACK, 0, "\2SAMPLE\0TIME",   menu_settings_cb},
-  { MT_CALLBACK, 0, "\2LO\0DRIVE",      menu_settings_cb},
-  { MT_SUBMENU,  0, S_RARROW" MORE",    menu_settings2},
+  { 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_SUBMENU,  0,                 S_RARROW" MORE",    menu_settings2},
+  { MT_CANCEL,   0,                 S_LARROW" BACK", NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
 
-//  { MT_SUBMENU,  0, "RBW", menu_rbw},
+
+static const menuitem_t menu_settingshigh2[] =
+{
+  { MT_CALLBACK, 0, "AGC",              menu_settings2_cb},
+  { MT_CALLBACK, 0, "LNA",              menu_settings2_cb},
   { MT_CANCEL,   0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
+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_SUBMENU,  0,                 S_RARROW" MORE",    menu_settingshigh2},
+  { MT_CANCEL,   0,                 S_LARROW" BACK", NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
+
+static const menuitem_t menu_calibrate[] =
+{
+ { MT_FORM | MT_TITLE,      0, "CONNECT INPUT AND OUTPUT",  NULL},
+ { MT_FORM | MT_CALLBACK,   0, "CALIBRATE",                 menu_calibrate_cb},
+ { MT_FORM | MT_CALLBACK,   0, "RESET CALBRATION",          menu_calibrate_cb},
+ { MT_FORM | MT_CANCEL,     0, S_LARROW" BACK",             NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
+
 static const menuitem_t menu_config[] = {
-  { MT_CALLBACK, 0, "\2TOUCH\0CAL",     menu_config_cb},
-  { MT_CALLBACK, 0, "\2TOUCH\0TEST",    menu_config_cb},
-  { MT_CALLBACK, 0, "\2SELF\0TEST",     menu_config_cb},
-  { MT_CALLBACK, 0, "VERSION",          menu_config_cb},
-  { MT_SUBMENU,  0, "SETTINGS",         menu_settings},
+  { MT_FORM | MT_CALLBACK, 0, "TOUCH CAL",     menu_config_cb},
+  { MT_FORM | MT_CALLBACK, 0, "TOUCH TEST",    menu_config_cb},
+  { MT_FORM | MT_CALLBACK, 0, "SELF TEST",     menu_config_cb},
+  { MT_FORM | MT_SUBMENU,  0, "CALIBRATE",     menu_calibrate},
+  { MT_FORM | MT_CALLBACK, 0, "VERSION",       menu_config_cb},
+//  { MT_SUBMENU,  0, "SETTINGS",         menu_settings},
 //  { MT_SUBMENU,  0, "RBW", menu_rbw},
-  { MT_SUBMENU,  0, S_RARROW"DFU",  menu_dfu},
-  { MT_CANCEL, 0, S_LARROW" BACK", NULL },
+  { MT_FORM | MT_SUBMENU,  0, S_RARROW"DFU",  menu_dfu},
+  { MT_FORM | MT_CANCEL, 0, S_LARROW" BACK", NULL },
   { MT_NONE,     0, NULL, NULL } // sentinel
 };
 
-static const menuitem_t menu_top[] = {
-  { MT_CALLBACK, 0, "AUTO",       menu_autosettings_cb},
-  { MT_SUBMENU,  0, "SCAN",       menu_stimulus},
-  { MT_SUBMENU,  0, "MARKER",     menu_marker},
-  { MT_SUBMENU,  0, "DISPLAY",    menu_scale},
-  { MT_SUBMENU,  0, "STORAGE",    menu_storage},
-  { MT_SUBMENU,  0, "MODE",       menu_mode},
-  { MT_SUBMENU,  0, "CONFIG",     menu_config},
+static const menuitem_t menu_mode[] = {
+  { MT_FORM | MT_TITLE,    0, "MODE",           NULL},
+  { MT_FORM | MT_CALLBACK, 0, "LOW INPUT",      menu_mode_cb},
+  { MT_FORM | MT_CALLBACK, 0, "HIGH INPUT",     menu_mode_cb},
+  { MT_FORM | MT_CALLBACK, 0, "LOW OUTPUT",     menu_mode_cb},
+  { MT_FORM | MT_CALLBACK, 0, "HIGH OUTPUT",    menu_mode_cb},
+  { MT_FORM | MT_SUBMENU,  0, "CAL OUTPUT: %s", menu_reffer},
+  { MT_FORM | MT_SUBMENU,  0, "CONFIG",         menu_config},
+//  { MT_CANCEL,   0, S_LARROW" BACK", NULL },
+  { MT_NONE,     0, NULL, NULL } // sentinel
+};
+
+const menuitem_t menu_top[] = {
+  { MT_CALLBACK, 0, "AUTO",         menu_autosettings_cb},
+  { MT_SUBMENU,  0, "SCAN",         menu_stimulus},
+  { MT_SUBMENU,  0, "MARKER",       menu_marker},
+  { MT_SUBMENU,  0, "DISPLAY",      menu_scale},
+  { MT_SUBMENU,  0, "STORAGE",      menu_storage},
+  { 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_CALLBACK, 0, "AUTO",         menu_autosettings_cb},
+  { MT_SUBMENU,  0, "SCAN",         menu_stimulus},
+  { MT_SUBMENU,  0, "MARKER",       menu_marker},
+  { MT_SUBMENU,  0, "DISPLAY",      menu_scalehigh},
+  { MT_SUBMENU,  0, "STORAGE",      menu_storage},
+  { MT_SUBMENU,  0, "SETTINGS",     menu_settingshigh},
+  { MT_CANCEL,   0, S_LARROW" MODE",NULL},
   { MT_NONE,     0, NULL, NULL } // sentinel,
  // MENUITEM_CLOSE,
 };
@@ -651,20 +838,38 @@ static const menuitem_t menu_top[] = {
 
 #define ACTIVE_COLOR RGBHEX(0x007FFF)
 
+void frequency_string(char *buf, size_t len, int32_t freq);
+
+int menu_is_form(const menuitem_t *menu)
+{
+  int i;
+  for (i = 0; MT_MASK(menu[i].type) != MT_NONE; i++)
+    if (menu[i].type & MT_FORM)
+      return (true);
+  return(false);
+}
+
+
 static void menu_item_modify_attribute(
     const menuitem_t *menu, int item, uint16_t *fg, uint16_t *bg)
 {
   if (menu == menu_mode) {
-    if (item == GetMode()){
+    if (item == GetMode()+1) {
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
+    } else if (item == 5) {
+      plot_printf(uistat.text, sizeof uistat.text, menu_reffer_text[get_refer_output()+1]);
     }
-  } else if (menu == menu_refer) {
+  } 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;
     }
-  } else if (menu == menu_refer2) {
+  } else if (menu == menu_reffer2) {
     if (item == get_refer_output() - 4){
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
@@ -675,10 +880,12 @@ static void menu_item_modify_attribute(
       *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;
     }
+#endif
   } else if (menu == menu_average) {
     if (item == GetAverage()){
       *bg = DEFAULT_MENU_TEXT_COLOR;
@@ -708,20 +915,64 @@ static void menu_item_modify_attribute(
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
     }
-  } else if (menu == menu_settings2) {
-    if (item ==0 && settingAGC){
+  } else if (menu == menu_settings2 || menu == menu_settingshigh2) {
+    if (item ==0 && setting_agc){
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
     }
-    if (item == 1 && settingLNA){
+    if (item == 1 && setting_lna){
       *bg = DEFAULT_MENU_TEXT_COLOR;
       *fg = config.menu_normal_color;
     }
-    if (item == 2 && extraVFO){
+    if (item == 2 && setting_tracking){         // should not happen in high mode
       *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;
+  }
 }
 
 static void fetch_numeric_target(void)
@@ -729,41 +980,61 @@ static void fetch_numeric_target(void)
   switch (keypad_mode) {
   case KM_START:
     uistat.value = get_sweep_frequency(ST_START);
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_STOP:
     uistat.value = get_sweep_frequency(ST_STOP);
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_CENTER:
     uistat.value = get_sweep_frequency(ST_CENTER);
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_SPAN:
     uistat.value = get_sweep_frequency(ST_SPAN);
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_CW:
     uistat.value = get_sweep_frequency(ST_CW);
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_SCALE:
     uistat.value = get_trace_scale(uistat.current_trace) * 1000;
+    plot_printf(uistat.text, sizeof uistat.text, "%ddB/", uistat.value / 1000);
     break;
   case KM_REFPOS:
     uistat.value = get_trace_refpos(uistat.current_trace) * 1000;
+    plot_printf(uistat.text, sizeof uistat.text, "%ddB", uistat.value / 1000);
     break;
   case KM_ATTENUATION:
-    uistat.value = settingAttenuate;
+    uistat.value = setting_attenuate;
+    plot_printf(uistat.text, sizeof uistat.text, "%ddB", uistat.value);
      break;
   case KM_ACTUALPOWER:
     uistat.value = settingLevelOffset();
+    plot_printf(uistat.text, sizeof uistat.text, "%ddB", uistat.value);
     break;
   case KM_IF:
     uistat.value = frequency_IF;
+    plot_printf(uistat.text, sizeof uistat.text, "%3.3fMHz", uistat.value / 1000000.0);
     break;
   case KM_SAMPLETIME:
-    uistat.value = settingSpeed;
+    uistat.value = setting_step_delay;
+    plot_printf(uistat.text, sizeof uistat.text, "%3duS", uistat.value);
     break;
   case KM_DRIVE:
-    uistat.value = settingDrive;
+    uistat.value = setting_drive;
+    plot_printf(uistat.text, sizeof uistat.text, "%3ddB", uistat.value);
+    break;
+  case KM_LOWOUTLEVEL:
+    uistat.value = setting_attenuate;
+    uistat.value = -5 - uistat.value;           // compensation for dB offset during low output mode
+    plot_printf(uistat.text, sizeof uistat.text, "%ddB", uistat.value);
+    break;
+  case KM_HIGHOUTLEVEL:
+    uistat.value = setting_drive*5 + 5;
+    plot_printf(uistat.text, sizeof uistat.text, "%3ddB", uistat.value);
     break;
-
   }
   
   {
@@ -776,7 +1047,6 @@ static void fetch_numeric_target(void)
 //  uistat.previous_value = uistat.value;
 }
 
-
 static void
 set_numeric_value(void)
 {
@@ -802,9 +1072,7 @@ set_numeric_value(void)
     set_trace_scale(2, uistat.value / 1000.0);
     break;
   case KM_REFPOS:
-    set_trace_refpos(0, NGRIDY - uistat.value / get_trace_scale(0));
-    set_trace_refpos(1, NGRIDY - uistat.value / get_trace_scale(0));
-    set_trace_refpos(2, NGRIDY - uistat.value / get_trace_scale(0));
+    SetRefpos(uistat.value);
     break;
   case KM_ATTENUATION:
     SetAttenuation(uistat.value);
@@ -814,14 +1082,22 @@ set_numeric_value(void)
     config_save();
     break;
   case KM_IF:
-    frequency_IF = uistat.value;
+    SetIF(uistat.value);
     config_save();
     break;
   case KM_SAMPLETIME:
-    settingSpeed = uistat.value;
+    SetStepDelay(uistat.value);
     break;
   case KM_DRIVE:
-    settingDrive = uistat.value;
+    SetDrive(uistat.value);
+    break;
+  case KM_LOWOUTLEVEL:
+    uistat.value = -5 - uistat.value ;           // compensation for dB offset during low output mode
+    SetAttenuation(uistat.value);
+    break;
+  case KM_HIGHOUTLEVEL:
+    uistat.value = uistat.value / 5 - 1 ;           // compensation for dB offset during high output mode
+    SetDrive(uistat.value);
     break;
   }
 }