diff --git a/Src/bldc.c b/Src/bldc.c
index 23b9afe..d5fb4c9 100644
--- a/Src/bldc.c
+++ b/Src/bldc.c
@@ -39,13 +39,14 @@ extern RT_MODEL *const rtM_Right;
 extern DW   rtDW_Left;                  /* Observable states */
 extern ExtU rtU_Left;                   /* External inputs */
 extern ExtY rtY_Left;                   /* External outputs */
+extern P    rtP_Left;
 
 extern DW   rtDW_Right;                 /* Observable states */
 extern ExtU rtU_Right;                  /* External inputs */
 extern ExtY rtY_Right;                  /* External outputs */
 // ###############################################################################
 
-static int16_t pwm_margin = 110;        /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */
+static int16_t pwm_margin;              /* This margin allows to have a window in the PWM signal for proper FOC Phase currents measurement */
 
 extern uint8_t ctrlModReq;
 static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV);
@@ -146,6 +147,13 @@ void DMA1_Channel1_IRQHandler(void) {
       buzzerPrev = 0;
   }
 
+  // Adjust pwm_margin depending on the selected Control Type
+  if (rtP_Left.z_ctrlTypSel == FOC_CTRL) {
+    pwm_margin = 110;
+  } else {
+    pwm_margin = 0;
+  }
+
   // ############################### MOTOR CONTROL ###############################
 
   int ul, vl, wl;