From 5eeffab5e5ae5b46d027a876e923a8d8b995c11e Mon Sep 17 00:00:00 2001
From: InsanityAutomation
 <38436470+InsanityAutomation@users.noreply.github.com>
Date: Sat, 23 Mar 2019 23:58:34 -0400
Subject: [PATCH] Fix BLTouch Menus (#13441)

---
 Marlin/src/feature/bltouch.cpp             |  1 +
 Marlin/src/feature/bltouch.h               |  4 +++-
 Marlin/src/lcd/menu/menu_configuration.cpp | 17 ++++++++++-------
 3 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp
index fc3757a47a..fdbab975e9 100644
--- a/Marlin/src/feature/bltouch.cpp
+++ b/Marlin/src/feature/bltouch.cpp
@@ -36,6 +36,7 @@ void stop();
 #include "../core/debug_out.h"
 
 void BLTouch::command(const BLTCommand cmd) {
+  //SERIAL_ECHOLNPAIR("BLTouch Command :", cmd);
   MOVE_SERVO(Z_PROBE_SERVO_NR, cmd);
   safe_delay(BLTOUCH_DELAY);
 }
diff --git a/Marlin/src/feature/bltouch.h b/Marlin/src/feature/bltouch.h
index c51d1b4a14..9e6a5c09fb 100644
--- a/Marlin/src/feature/bltouch.h
+++ b/Marlin/src/feature/bltouch.h
@@ -44,13 +44,15 @@ public:
   FORCE_INLINE static void set_5V_mode() { command(BLTOUCH_5V_MODE); }
   FORCE_INLINE static void set_OD_mode() { command(BLTOUCH_OD_MODE); }
   FORCE_INLINE static void set_SW_mode() { command(BLTOUCH_SW_MODE); }
+  FORCE_INLINE static void selftest()    { command(BLTOUCH_SELFTEST); }
 
   FORCE_INLINE static bool deploy() { return set_deployed(true); }
   FORCE_INLINE static bool stow()   { return set_deployed(false); }
 
-private:
   FORCE_INLINE static void _deploy()     { command(BLTOUCH_DEPLOY); }
   FORCE_INLINE static void _stow()       { command(BLTOUCH_STOW); }
+
+private:
   static bool set_deployed(const bool deploy);
 };
 
diff --git a/Marlin/src/lcd/menu/menu_configuration.cpp b/Marlin/src/lcd/menu/menu_configuration.cpp
index 2f19c22a08..d6115f5501 100644
--- a/Marlin/src/lcd/menu/menu_configuration.cpp
+++ b/Marlin/src/lcd/menu/menu_configuration.cpp
@@ -42,6 +42,9 @@
 
 #if HAS_BED_PROBE
   #include "../../module/probe.h"
+  #if ENABLED(BLTOUCH)
+    #include "../../feature/bltouch.h"
+  #endif
 #endif
 
 #define HAS_DEBUG_MENU ENABLED(LCD_PROGRESS_BAR_TEST)
@@ -157,14 +160,14 @@ static void lcd_factory_settings() {
   void menu_bltouch() {
     START_MENU();
     MENU_BACK(MSG_MAIN);
-    MENU_ITEM(gcode, MSG_BLTOUCH_RESET, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_RESET)));
-    MENU_ITEM(gcode, MSG_BLTOUCH_SELFTEST, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_SELFTEST)));
-    MENU_ITEM(gcode, MSG_BLTOUCH_DEPLOY, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_DEPLOY)));
-    MENU_ITEM(gcode, MSG_BLTOUCH_STOW, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_STOW)));
+    MENU_ITEM(function, MSG_BLTOUCH_RESET, bltouch.reset);
+    MENU_ITEM(function, MSG_BLTOUCH_SELFTEST, bltouch.selftest);
+    MENU_ITEM(function, MSG_BLTOUCH_DEPLOY, bltouch._deploy);
+    MENU_ITEM(function, MSG_BLTOUCH_STOW, bltouch._stow);
     #if ENABLED(BLTOUCH_V3)
-      MENU_ITEM(gcode, MSG_BLTOUCH_SW_MODE, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_SW_MODE)));
-      MENU_ITEM(gcode, MSG_BLTOUCH_5V_MODE, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_5V_MODE)));
-      MENU_ITEM(gcode, MSG_BLTOUCH_OD_MODE, PSTR("M280 P" STRINGIFY(Z_PROBE_SERVO_NR) " S" STRINGIFY(BLTOUCH_OD_MODE)));
+      MENU_ITEM(function, MSG_BLTOUCH_SW_MODE, bltouch.set_SW_mode);
+      MENU_ITEM(function, MSG_BLTOUCH_5V_MODE, bltouch.set_5V_mode);
+      MENU_ITEM(function, MSG_BLTOUCH_OD_MODE, bltouch.set_OD_mode);
     #endif
     END_MENU();
   }