diff --git a/Inc/config.h b/Inc/config.h
index 21df774..31b4096 100644
--- a/Inc/config.h
+++ b/Inc/config.h
@@ -124,6 +124,10 @@
    Outputs:
     - speedR and speedL: normal driving INPUT_MIN to INPUT_MAX
 */
+// Enable/Disable Motor
+#define MOTOR_LEFT_ENA                  // [-] Enable LEFT motor.  Comment-out if this motor is not needed to be operational
+#define MOTOR_RIGHT_ENA                 // [-] Enable RIGHT motor. Comment-out if this motor is not needed to be operational
+
 // Control selections
 #define CTRL_TYP_SEL    2               // [-] Control type selection: 0 = Commutation , 1 = Sinusoidal, 2 = FOC Field Oriented Control (default)
 #define CTRL_MOD_REQ    1               // [-] Control mode request: 0 = Open mode, 1 = VOLTAGE mode (default), 2 = SPEED mode, 3 = TORQUE mode. Note: SPEED and TORQUE modes are only available for FOC!
diff --git a/Src/bldc.c b/Src/bldc.c
index 680d70f..c992b6d 100644
--- a/Src/bldc.c
+++ b/Src/bldc.c
@@ -171,7 +171,9 @@ void DMA1_Channel1_IRQHandler(void) {
     rtU_Left.i_DCLink     = curL_DC;    
     
     /* Step the controller */
+    #ifdef MOTOR_LEFT_ENA    
     BLDC_controller_step(rtM_Left);
+    #endif
 
     /* Get motor outputs here */
     ul            = rtY_Left.DC_phaA;
@@ -206,7 +208,9 @@ void DMA1_Channel1_IRQHandler(void) {
     rtU_Right.i_DCLink      = curR_DC;
 
     /* Step the controller */
+    #ifdef MOTOR_RIGHT_ENA
     BLDC_controller_step(rtM_Right);
+    #endif
 
     /* Get motor outputs here */
     ur            = rtY_Right.DC_phaA;
diff --git a/Src/control.c b/Src/control.c
index 7c5b4cc..f917c7d 100644
--- a/Src/control.c
+++ b/Src/control.c
@@ -86,17 +86,32 @@ void PPM_Init(void) {
 
 
 #ifdef CONTROL_PWM
+ /*
+  * Illustration of the PWM functionality
+  * CH1 ________|‾‾‾‾‾‾‾‾‾‾|________
+  * CH2 ______________|‾‾‾‾‾‾‾‾‾‾‾|________
+  *             ↑     ↑    ↑      ↑
+  * TIM2       RST  SAVE RC_CH1 RC_CH1
+ */
+
 uint16_t pwm_captured_ch1_value = 500;
 uint16_t pwm_captured_ch2_value = 500;
+uint16_t pwm_CNT_prev_ch1 = 0;
+uint16_t pwm_CNT_prev_ch2 = 0;
 uint32_t pwm_timeout_ch1 = 0;
 uint32_t pwm_timeout_ch2 = 0;
 
 void PWM_ISR_CH1_Callback(void) {
   // Dummy loop with 16 bit count wrap around
-  if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2)) {   // Rising  Edge interrupt -> reset timer
-    TIM2->CNT = 0;
+  if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2)) {   // Rising  Edge interrupt -> save timer value OR reset timer
+    if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3)) {
+      pwm_CNT_prev_ch1 = TIM2->CNT;
+    } else {
+      TIM2->CNT = 0;
+      pwm_CNT_prev_ch1 = 0;
+    }
   } else {                                    // Falling Edge interrupt -> measure pulse duration
-    uint16_t rc_signal = TIM2->CNT;
+    uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch1;
     if (IN_RANGE(rc_signal, 900, 2100)){
       timeout = 0;
       pwm_timeout_ch1 = 0;
@@ -107,10 +122,15 @@ void PWM_ISR_CH1_Callback(void) {
 
 void PWM_ISR_CH2_Callback(void) {
   // Dummy loop with 16 bit count wrap around
-  if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3)) {   // Rising  Edge interrupt -> reset timer
-    TIM2->CNT = 0;
+  if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3)) {   // Rising  Edge interrupt -> save timer value OR reset timer
+    if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2)) {
+      pwm_CNT_prev_ch2 = TIM2->CNT;
+    } else {
+      TIM2->CNT = 0;
+      pwm_CNT_prev_ch2 = 0;
+    }
   } else {                                    // Falling Edge interrupt -> measure pulse duration
-    uint16_t rc_signal = TIM2->CNT;
+    uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch2;
     if (IN_RANGE(rc_signal, 900, 2100)){
       timeout = 0;
       pwm_timeout_ch2 = 0;