diff --git a/main.c b/main.c
index 1008108..5b8c824 100644
--- a/main.c
+++ b/main.c
@@ -80,10 +80,8 @@ static THD_FUNCTION(Thread1, arg)
 
       chMtxLock(&mutex);
       ui_process();
-      chMtxUnlock(&mutex);
 
       if (sweep_enabled) {
-        chMtxLock(&mutex);
         if (vbat != -1) {
           adc_stop(ADC1);
           vbat = adc_vbat_read(ADC1);
@@ -96,12 +94,11 @@ static THD_FUNCTION(Thread1, arg)
           plot_into_index(measured);
           redraw_request |= REDRAW_CELLS;
         }
-
-        /* plot trace and other indications as raster */
-        draw_all(completed); // flush markmap only if scan completed to prevent remaining traces
-
-        chMtxUnlock(&mutex);
       }
+
+      /* plot trace and other indications as raster */
+      draw_all(completed); // flush markmap only if scan completed to prevent remaining traces
+      chMtxUnlock(&mutex);
     }
 }
 
@@ -765,8 +762,10 @@ static void set_frequencies(uint32_t start, uint32_t stop, int16_t points)
 {
   int i;
   float span = stop - start;
-  for (i = 0; i < points; i++)
-    frequencies[i] = start + i * span / (float)(points - 1);
+  for (i = 0; i < points; i++) {
+    float offset = i * span / (float)(points - 1);
+    frequencies[i] = start + (uint32_t)offset;
+  }
   // disable at out of sweep range
   for (; i < sweep_points; i++)
     frequencies[i] = 0;
diff --git a/plot.c b/plot.c
index fb9f922..edda534 100644
--- a/plot.c
+++ b/plot.c
@@ -942,11 +942,15 @@ search_index_range_x(int x, uint32_t index[101], int *i0, int *i1)
     i = (head + tail) / 2;
     if (x == CELL_X0(index[i]))
       break;
-
-    if (x < CELL_X0(index[i]))
-      tail = i+1;
-    else
+    else if (x < CELL_X0(index[i])) {
+      if (tail == i+1)
+        break;
+      tail = i+1;      
+    } else {
+      if (head == i)
+        break;
       head = i;
+    }
   }
 
   if (x != CELL_X0(index[i]))
diff --git a/ui.c b/ui.c
index 765554a..938ed9a 100644
--- a/ui.c
+++ b/ui.c
@@ -599,15 +599,17 @@ menu_trace_cb(int item)
   if (item < 0 || item >= 4)
     return;
   if (trace[item].enabled) {
-    trace[item].enabled = FALSE;
-    choose_active_trace();
+    if (item == uistat.current_trace) {
+      // disable if active trace is selected
+      trace[item].enabled = FALSE;
+      choose_active_trace();
+    } else {
+      // make active selected trace
+      uistat.current_trace = item;
+    }
   } else {
     trace[item].enabled = TRUE;
     uistat.current_trace = item;
-    //menu_move_back();
-    //request_to_redraw_grid();
-    //ui_mode_normal();
-    //redraw_all();
   }
   request_to_redraw_grid();
   draw_menu();
@@ -844,15 +846,38 @@ menu_marker_op_cb(int item)
   //redraw_all();
 }
 
+void 
+active_marker_select(int item)
+{
+  if (item == -1) {
+    active_marker = previous_marker;
+    previous_marker = -1;
+    if (active_marker == -1) {
+      choose_active_marker();
+    }
+  } else {
+    if (previous_marker != active_marker)
+      previous_marker = active_marker;
+    active_marker = item;
+  }
+}
+
 static void
 menu_marker_sel_cb(int item)
 {
   if (item >= 0 && item < 4) {
-    // enable specified marker
-    markers[item].enabled = TRUE;
-    if (previous_marker != active_marker)
-      previous_marker = active_marker;
-    active_marker = item;
+    if (markers[item].enabled) {
+      if (item == active_marker) {
+        // disable if active trace is selected
+        markers[item].enabled = FALSE;
+        active_marker_select(-1);
+      } else {
+        active_marker_select(item);
+      }
+    } else {
+      markers[item].enabled = TRUE;
+      active_marker_select(item);
+    }
   } else if (item == 4) { /* all off */
       markers[0].enabled = FALSE;
       markers[1].enabled = FALSE;
@@ -861,10 +886,8 @@ menu_marker_sel_cb(int item)
       previous_marker = -1;
       active_marker = -1;      
   }
-  if (active_marker >= 0)
-    redraw_marker(active_marker, TRUE);
+  redraw_marker(active_marker, TRUE);
   draw_menu();
-  //ui_mode_normal();
 }
 
 const menuitem_t menu_calop[] = {