diff --git a/src/hal/user_comps/xhc-whb04b-6/hal.cc b/src/hal/user_comps/xhc-whb04b-6/hal.cc
index 8d12e0ab559..e191a7bd87a 100644
--- a/src/hal/user_comps/xhc-whb04b-6/hal.cc
+++ b/src/hal/user_comps/xhc-whb04b-6/hal.cc
@@ -1324,8 +1324,9 @@ void Hal::setJogCounts(const HandWheelCounters& counters)
     *memory->out.axisAJogCounts = counters.counts(HandWheelCounters::CounterNameToIndex::AXIS_A);
     *memory->out.axisBJogCounts = counters.counts(HandWheelCounters::CounterNameToIndex::AXIS_B);
     *memory->out.axisCJogCounts = counters.counts(HandWheelCounters::CounterNameToIndex::AXIS_C);
-        requestManualMode(false);
-        requestTeleopMode(false);
+    
+    requestManualMode(false);
+    requestTeleopMode(false);
 }
 // ----------------------------------------------------------------------
 void Hal::setFunction(bool enabled)
@@ -1340,6 +1341,11 @@ bool Hal::requestAutoMode(bool isRisingEdge)
 // ----------------------------------------------------------------------
 bool Hal::requestManualMode(bool isRisingEdge)
 {
+    if(isRisingEdge && *memory->in.isModeAuto)
+    {
+        //Don't try to change from auto to manual
+        return false;
+    }
     return requestMode(isRisingEdge, memory->out.doModeManual, memory->in.isModeManual);
 }
 // ----------------------------------------------------------------------
diff --git a/src/hal/user_comps/xhc-whb04b-6/pendant.cc b/src/hal/user_comps/xhc-whb04b-6/pendant.cc
index d1fe8969064..4c22736d30a 100644
--- a/src/hal/user_comps/xhc-whb04b-6/pendant.cc
+++ b/src/hal/user_comps/xhc-whb04b-6/pendant.cc
@@ -586,6 +586,11 @@ void FeedRotaryButton::setStepMode(HandwheelStepmodes::Mode stepMode)
     update();
 }
 // ----------------------------------------------------------------------
+void FeedRotaryButton::setStepMode_5_10(bool enable)
+{
+    mIsStepMode_5_10 = enable;
+}
+// ----------------------------------------------------------------------
 HandwheelStepmodes::Mode FeedRotaryButton::stepMode() const
 {
     return mStepMode;
@@ -625,7 +630,7 @@ void FeedRotaryButton::update()
         mStepSize    = mStepSizeMapper.getStepSize(second);
         mIsPermitted = mStepSizeMapper.isPermitted(second);
         
-        if (mIsStepMode_5_10 && mStepSize > 2) {mStepSize    = 0;}             // TODO DOES NOT WORK bool variable seems to be not synched inside pendant.h
+        if (!mIsStepMode_5_10 && mStepSize > 2) {mStepSize    = 0;}
         
     }
     else if (mStepMode == HandwheelStepmodes::Mode::CON)
@@ -730,11 +735,6 @@ void Handwheel::enableVerbose(bool enable)
     }
 }
 // ----------------------------------------------------------------------
-void Handwheel::setMode(HandWheelCounters::CounterNameToIndex activeCounterMode)
-{
-    mCounters.setActiveCounter(activeCounterMode);
-}
-// ----------------------------------------------------------------------
 void Handwheel::count(int8_t delta)
 {
     assert(mEventListener != nullptr);
@@ -1475,11 +1475,17 @@ bool Pendant::onJogDialEvent(const HandWheelCounters& counters, int8_t delta)
                    {
                        mHal.toggleFeedrateDecrease();
                    }
-             }
-             else if (!counters.isLeadCounterActive() && (feedButton.stepMode() == HandwheelStepmodes::Mode::CON || feedButton.stepMode() == HandwheelStepmodes::Mode::STEP))
-             {      // Normal Mode
-                    mHal.setJogCounts(counters);
-             }
+            }
+            
+            if (!counters.isLeadCounterActive())
+            {
+                //The counters must be set always if not in lead mode
+                //Otherwhise, the machine will move, sometimes a long distance in the following case:
+                //MGP mode -> Wheel turned -> CON or STEP mode -> After first wheel pulse
+                //due to the counters are increased in MPG mode but not set until the first wheel count event
+                //Setting them does not create a move in MPG mode due to the scale is zero
+                mHal.setJogCounts(counters);
+            }
         }
         mDisplay.onJogDialEvent(counters, delta);
         return true;
@@ -1563,19 +1569,19 @@ void Pendant::dispatchAxisEventToHal(const KeyCode& axis, bool isActive)
     }
 }
 // ----------------------------------------------------------------------
-void Pendant::setLeadModeSpindle(bool /*enable*/)
+void Pendant::setLeadModeSpindle(bool enable)
 {
-    mIsLeadModeSpindle = true;
+    mIsLeadModeSpindle = enable;
 }
 // ----------------------------------------------------------------------
-void Pendant::setLeadModeFeed(bool /*enable*/)
+void Pendant::setLeadModeFeed(bool enable)
 {
-    mIsLeadModeFeed = true;
+    mIsLeadModeFeed = enable;
 }
 // ----------------------------------------------------------------------
-void Pendant::setStepMode_5_10(bool /*enable*/)
+void Pendant::setStepMode_5_10(bool enable)
 {
-    mIsStepMode_5_10 = true;
+    mCurrentButtonsState.feedButton().setStepMode_5_10(enable);
 }
 // ----------------------------------------------------------------------
 Display::Display(const ButtonsState& currentButtonsState, Hal& hal, UsbOutPackageData& displayData) :
diff --git a/src/hal/user_comps/xhc-whb04b-6/pendant.h b/src/hal/user_comps/xhc-whb04b-6/pendant.h
index 53c121cd1cd..a1060e005fd 100644
--- a/src/hal/user_comps/xhc-whb04b-6/pendant.h
+++ b/src/hal/user_comps/xhc-whb04b-6/pendant.h
@@ -393,6 +393,7 @@ class FeedRotaryButton : public RotaryButton
     ~FeedRotaryButton();
     virtual bool setKeyCode(const KeyCode& keyCode) override;
     void setStepMode(HandwheelStepmodes::Mode stepMode);
+    void setStepMode_5_10(bool enable);
     HandwheelStepmodes::Mode stepMode() const;
     float stepSize() const;
     bool isPermitted() const override;
@@ -440,7 +441,6 @@ class Handwheel
     Handwheel(const FeedRotaryButton& feedButton, KeyEventListener* listener = nullptr);
     ~Handwheel();
     void enableVerbose(bool enable);
-    void setMode(HandWheelCounters::CounterNameToIndex mode);
     void count(int8_t delta);
     const HandWheelCounters& counters() const;
     HandWheelCounters& counters();
@@ -588,7 +588,6 @@ class Pendant : public KeyEventListener
     //! if in Lead mode: if true jog wheel changes the spindle speed, changes the feed override otherwise
     bool         mIsLeadModeSpindle = false;
     bool         mIsLeadModeFeed = false;
-    bool         mIsStepMode_5_10 = false;
 
     // float mScale;
     // float mMaxVelocity;