diff --git a/nanovna.h b/nanovna.h
index 121b3fd..a73d516 100644
--- a/nanovna.h
+++ b/nanovna.h
@@ -740,6 +740,9 @@ int current_menu_is_form(void);
 void menu_mode_cb(int, uint8_t);
 void ui_mode_normal(void);
 void ui_mode_menu(void);
+void menu_push_lowoutput(void);
+void menu_push_highoutput(void);
+void menu_move_top(void);
 
 // Irq operation process set
 #define OP_NONE       0x00
diff --git a/sa_cmd.c b/sa_cmd.c
index f8c8fd6..da1d8a8 100644
--- a/sa_cmd.c
+++ b/sa_cmd.c
@@ -1,5 +1,4 @@
-/* Copyright (c) 2020, Erik Kaashoek erik@kaashoek.com
- * All rights reserved.
+/* All rights reserved.
  *
  * This is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -37,19 +36,24 @@ VNA_SHELL_FUNCTION(cmd_mode)
   int io = get_str_index(argv[1], cmd_in_out);
   if (lh<0 || io<0)
     goto usage;
+  menu_move_top();
   switch(lh+io*2)
   {
   case 0:
     set_mode(M_LOW);
+    ui_mode_normal();
     break;
   case 1:
     set_mode(M_HIGH);
+    ui_mode_normal();
     break;
   case 2:
     set_mode(M_GENLOW);
+    menu_push_lowoutput();
     break;
   case 3:
     set_mode(M_GENHIGH);
+    menu_push_highoutput();
     break;
   }
 }
diff --git a/sa_core.c b/sa_core.c
index 15d7f61..9ed5eae 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -1148,12 +1148,15 @@ float perform(bool break_on_operation, int i, uint32_t f, int tracking)
       SI4432_Drive(d);
       if (a > 0)
         a = 0;
+
       if( a >  - SWITCH_ATTENUATION) {
         set_switch_transmit();
       } else {
         a = a + SWITCH_ATTENUATION;
         set_switch_receive();
       }
+      if (a < -31)
+        a = -31;
       a = -a;
       PE4302_Write_Byte((int)(a * 2) );
     }
diff --git a/ui.c b/ui.c
index 2ac502b..b0bb6d7 100644
--- a/ui.c
+++ b/ui.c
@@ -129,7 +129,6 @@ static void erase_menu_buttons(void);
 static void ui_process_keypad(void);
 static void ui_process_numeric(void);
 static void choose_active_marker(void);
-
 static void menu_move_back(void);
 static void menu_push_submenu(const menuitem_t *submenu);
 //static const menuitem_t menu_marker_type[];
@@ -1193,6 +1192,18 @@ menu_push_submenu(const menuitem_t *submenu)
   draw_menu();
 }
 
+void
+menu_push_lowoutput(void)
+{
+  menu_push_submenu(menu_lowoutputmode);
+}
+
+void
+menu_push_highoutput(void)
+{
+  menu_push_submenu(menu_highoutputmode);
+}
+
 int current_menu_is_form(void)
 {
   return menu_is_form(menu_stack[menu_current_level]);
diff --git a/ui_sa.c b/ui_sa.c
index 8623b1f..578573f 100644
--- a/ui_sa.c
+++ b/ui_sa.c
@@ -1933,3 +1933,11 @@ set_numeric_value(void)
     break;
   }
 }
+
+void
+menu_move_top(void)
+{
+  while (menu_current_level > 0)
+    menu_move_back();
+}
+