Adding menu for Laser cut calibration

Hello,
I run Marlin bugfix branch 1.1.x.
My printer is a Prusa clone which I have mounted from aliexpress pieces, and now I am playing with a laser diode for plywood cutting.
I need often to produce the same gcode except for the speed and the pwm power, so I decided to add a menu in the "prepare" section for laser power and speed calibration.

The goal is to ask my printer to draw a 20mm square at a user defined speed and laser power to learn for each material to cut how many passes, z-offset value for each pass run, global speed and power of laser.

Here is the gcode I want to execute:

"G90\nG21\nG1 F5000\nG1 X0 Y20\nG4 P0\nM106 S%i\nG4 P0\nG1 F%i\nG1 X20 Y20\nG1 X20 Y0\nG1 X0 Y0\nG1 X0 Y20\nG4 P0\nM107 S0\nG1 F5000\nG1 X0 Y0"

with first %i = lasertest_power,
and second %i = lasertest_speed.

So here is the code I added to ultralcd.cpp today for testing,

in the declaration section:

  void lcd_lasercutprepare_menu();
  static int  lasertest_power = 255; //default values for power
  static int  lasertest_speed = 150; //default value for speed
  static char lasertest_gcode[150];

in lcd_prepare_menu() function:

  MENU_ITEM(submenu, _UxGT("Calibrate Laser cut"), lcd_lasercutprepare_menu);

then I defined my own menu function and my own callback:

void lasertest_callback(){
  sprintf_P(lasertest_code, PSTR("G90\nG21\nG1 F5000\nG1 X0 Y20\nG4 P0\nM106 S%i\nG4 P0\nG1 F%i\nG1 X20 Y20\nG1 X20 Y0\nG1 X0 Y0\nG1 X0 Y20\nG4 P0\nM107 S0\nG1 F5000\nG1 X0 Y0"), lasertest_power, lasertest_speed);
  enqueue_and_echo_commands_P(lasertest_code);
}

void lcd_lasercutprepare_menu() {
    START_MENU();
    MENU_BACK(MSG_PREPARE);
    MENU_ITEM(gcode, _UxGT("auto home head"), PSTR("G28\nG1 Z50"));                //for initializing the laser head position at (0,0,50)
    MENU_MULTIPLIER_ITEM_EDIT(int3, _UxGT("Power: "), &lasertest_power, 0, 255);   //for editing the power of the laser
    MENU_MULTIPLIER_ITEM_EDIT(int3, _UxGT("Speed: "), &lasertest_speed, 0, 5000);  //for editing the speed
    MENU_ITEM(gcode, _UxGT("Z.offset -= 0.1"), PSTR("G91\nG1 Z-0.1\nG90"));        //for move Z axis a little to try another pass
    MENU_ITEM(function, _UxGT("--Run test--"), lasertest_callback);                //for running one pass of the calibration test
    END_MENU();
  }

When I send the whole big string with power and speed in text characters, all is done correctly.
But when I execute my callback for fixing the speed and the power programmatically, some gcode commands are lost.

So, what is the best and simpliest way to achieve that?

I finally got it running.

Here is the diff file, if you want to implement it in your printer.
Currently, the menu texts are hardcoded, because it is a local patch in my code.

--- ultralcd.cpp	2018-03-24 23:56:39.000000000 +0100
+++ ultralcd.cpp	2018-03-24 23:42:23.000000000 +0100
@@ -187,6 +187,10 @@
   void lcd_control_temperature_menu();
   void lcd_control_motion_menu();
 
+  void lcd_lasercutprepare_menu();
+  static int lasertest_power = 255; //default cut laser power
+  static int lasertest_speed = 200; //default cut speed
+  
   #if DISABLED(SLIM_LCD_MENUS)
     void lcd_control_temperature_preheat_material1_settings_menu();
     void lcd_control_temperature_preheat_material2_settings_menu();
@@ -2574,6 +2578,8 @@
     //
     MENU_BACK(MSG_MAIN);
 
+    MENU_ITEM(submenu, _UxGT("Calibrate Laser cut"), lcd_lasercutprepare_menu);
+
     //
     // Move Axis
     
@@ -3185,6 +3191,39 @@
     END_MENU();
   }
 
+/*
+ * Callback called on push of [run one pass] menu entry.
+ * One pass consist on the laser cutting of a 20mm square
+ * Beware that in my configuration, I am using M106 and M107 to drive laser power. 
+ * If you are using others commands (like M4 or M5, substitute it in the following gcode).
+ */  
+void lasertest_callback(){
+  char s[15];
+  enqueue_and_echo_commands_P(PSTR("G90\nG21"));
+  sprintf_P(s, PSTR("M106 S%i"), lasertest_power); lcd_enqueue_command(s);
+  sprintf_P(s, PSTR("G1 F%i"), lasertest_speed); lcd_enqueue_command(s);  
+  enqueue_and_echo_commands_P(PSTR("G1 X0 Y20\nG1 X20 Y20\nG1 X20 Y0\nG1 X0 Y0\nM107 S0"));
+}
+
+/*
+ * MENU for calibrating laser cut for current material.
+ * The goal of this menu is to bring tools for determine the correct power/speed/nb of passes for a given material.
+ * Following entries are displayed in this menu:
+ * - Auto home +50 : the head is first homed then laser head goes up 50mm to its focalized position.
+ * - Disable steppers : if you want to move the bed by hand to place the laser head on an arbitrary position.
+ * - Speed (0-999 mm/min)      : test speed
+ * - Power (0-255)              : test power
+ * - Z.offset -= 0.1         : Clicking on this menu entry makes Z axis to move 0.1mm down
+ * - Start square test          : Actually starts the test = the gcode of a 20mm^2 square
+ */
+void lcd_lasercutprepare_menu() { 
+    START_MENU();
+    MENU_BACK(MSG_PREPARE);
+    MENU_ITEM(gcode, _UxGT("Auto home +50"), PSTR("G28\nG1 Z50"));
+    MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
+    MENU_MULTIPLIER_ITEM_EDIT(int3, _UxGT("Power: "), &lasertest_power, 0, 255);
+    MENU_MULTIPLIER_ITEM_EDIT(int3, _UxGT("Speed: "), &lasertest_speed, 0, 999);
+    MENU_ITEM(gcode, _UxGT("Z.offset -= 0.1"), PSTR("G91\nG1 Z-0.1\nG90"));
+    MENU_ITEM(function, _UxGT("[Run one pass]"), lasertest_callback);
+    END_MENU();
+  }
+  
   /**
    *
    * "Control" submenu