Levelsweep warp error and show UI after cmd mode switch

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

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;
   }
 }

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) );
     }

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]);

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();
+}
+