Servo::detach() doesn't affect another servos sharing same hardware t…

…imer
particle-iot · Feb 5, 2016 · f970923 · f970923
1 parent 4e72ca9
commit f970923
Show file tree

Hide file tree

Showing 3 changed files with 98 additions and 7 deletions.
diff --git a/hal/src/core/servo_hal.c b/hal/src/core/servo_hal.c
@@ -27,6 +27,7 @@
 #include "servo_hal.h"
 #include "gpio_hal.h"
 #include "pinmap_impl.h"
+#include "timer_info.h"
 
 /* Private typedef -----------------------------------------------------------*/
 
@@ -103,6 +104,10 @@ void HAL_Servo_Attach(uint16_t pin)
 
   TIM_ARRPreloadConfig(PIN_MAP[pin].timer_peripheral, ENABLE);
 
+  // Set channel flag
+  STM32_Advanced_Timer_Info* timer_info = (STM32_Advanced_Timer_Info*)STM32_Timer_Info_For_Pin(pin);
+  timer_info->active_channel_flags |= STM32_Timer_Channel_Mask(PIN_MAP[pin].timer_ch);
+
   // TIM enable counter
   TIM_Cmd(PIN_MAP[pin].timer_peripheral, ENABLE);
 
@@ -112,8 +117,36 @@ void HAL_Servo_Attach(uint16_t pin)
 
 void HAL_Servo_Detach(uint16_t pin)
 {
-  // TIM disable counter
-  TIM_Cmd(PIN_MAP[pin].timer_peripheral, DISABLE);
+  const STM32_Pin_Info* pin_info = HAL_Pin_Map() + pin;
+
+  // Reset channel's CCR register to 0 (no duty cycle)
+  switch (pin_info->timer_ch)
+  {
+    case TIM_Channel_1:
+      TIM_SetCompare1(pin_info->timer_peripheral, 0);
+      break;
+    case TIM_Channel_2:
+      TIM_SetCompare2(pin_info->timer_peripheral, 0);
+      break;
+    case TIM_Channel_3:
+      TIM_SetCompare3(pin_info->timer_peripheral, 0);
+      break;
+    case TIM_Channel_4:
+      TIM_SetCompare4(pin_info->timer_peripheral, 0);
+      break;
+    default:
+      break;
+  }
+
+  // Clear channel flag
+  STM32_Advanced_Timer_Info* timer_info = (STM32_Advanced_Timer_Info*)STM32_Timer_Info_For_Pin(pin);
+  timer_info->active_channel_flags &= ~(STM32_Timer_Channel_Mask(pin_info->timer_ch));
+
+  if (!timer_info->active_channel_flags)
+  {
+    // TIM disable counter
+    TIM_Cmd(pin_info->timer_peripheral, DISABLE);
+  }
 }
 
 void HAL_Servo_Write_Pulse_Width(uint16_t pin, uint16_t pulseWidth)

diff --git a/hal/src/stm32f2xx/servo_hal.c b/hal/src/stm32f2xx/servo_hal.c
@@ -27,6 +27,7 @@
 #include "servo_hal.h"
 #include "gpio_hal.h"
 #include "pinmap_impl.h"
+#include "timer_info.h"
 
 /* Private typedef -----------------------------------------------------------*/
 
@@ -129,6 +130,10 @@ void HAL_Servo_Attach(uint16_t pin)
 
     TIM_ARRPreloadConfig(PIN_MAP[pin].timer_peripheral, ENABLE);
 
+    // Set channel flag
+    STM32_Advanced_Timer_Info* timer_info = (STM32_Advanced_Timer_Info*)STM32_Timer_Info_For_Pin(pin);
+    timer_info->active_channel_flags |= STM32_Timer_Channel_Mask(PIN_MAP[pin].timer_ch);
+
     // TIM enable counter
     TIM_Cmd(PIN_MAP[pin].timer_peripheral, ENABLE);
 
@@ -142,9 +147,36 @@ void HAL_Servo_Attach(uint16_t pin)
 
 void HAL_Servo_Detach(uint16_t pin)
 {
-    // TIM disable counter
-    STM32_Pin_Info* PIN_MAP = HAL_Pin_Map();
-    TIM_Cmd(PIN_MAP[pin].timer_peripheral, DISABLE);
+    const STM32_Pin_Info* pin_info = HAL_Pin_Map() + pin;
+
+    // Reset channel's CCR register to 0 (no duty cycle)
+    switch (pin_info->timer_ch)
+    {
+        case TIM_Channel_1:
+            TIM_SetCompare1(pin_info->timer_peripheral, 0);
+            break;
+        case TIM_Channel_2:
+            TIM_SetCompare2(pin_info->timer_peripheral, 0);
+            break;
+        case TIM_Channel_3:
+            TIM_SetCompare3(pin_info->timer_peripheral, 0);
+            break;
+        case TIM_Channel_4:
+            TIM_SetCompare4(pin_info->timer_peripheral, 0);
+            break;
+        default:
+            break;
+    }
+
+    // Clear channel flag
+    STM32_Advanced_Timer_Info* timer_info = (STM32_Advanced_Timer_Info*)STM32_Timer_Info_For_Pin(pin);
+    timer_info->active_channel_flags &= ~(STM32_Timer_Channel_Mask(pin_info->timer_ch));
+
+    if (!timer_info->active_channel_flags)
+    {
+        // TIM disable counter
+        TIM_Cmd(pin_info->timer_peripheral, DISABLE);
+    }
 }
 
 void HAL_Servo_Write_Pulse_Width(uint16_t pin, uint16_t pulseWidth)

diff --git a/user/tests/wiring/no_fixture/servo.cpp b/user/tests/wiring/no_fixture/servo.cpp
@@ -28,9 +28,9 @@
 #include "unit-test/unit-test.h"
 
 #if defined(STM32F2XX)
-static pin_t pin = D0;//pin under test
+static pin_t pin = D0, pin2 = D1; // Pins sharing the same hardware timer
 #else
-static pin_t pin = A0;//pin under test
+static pin_t pin = A0, pin2 = A1;
 #endif
 
 test(SERVO_CannotAttachWhenPinSelectedIsNotTimerChannel) {
@@ -76,3 +76,29 @@ test(SERVO_WritePulseWidthOnPinResultsInCorrectMicroSeconds) {
     assertEqual(readPulseWidth, pulseWidth);
     //To Do : Add test for remaining pins if required
 }
+
+test(SERVO_DetachDoesntAffectAnotherServoUsingSameTimer) {
+    const int pulseWidth = 2000;
+    // Attach 1st servo
+    Servo servo1;
+    servo1.attach(pin);
+    servo1.writeMicroseconds(pulseWidth);
+    // Attach 2nd servo
+    Servo servo2;
+    servo2.attach(pin2);
+    servo2.writeMicroseconds(pulseWidth);
+    // Detach 1st servo
+    servo1.detach();
+    // Ensure 2nd servo is not affected
+    int readPulseWidth = 0;
+    const uint32_t start = millis();
+    for (int i = 0; i < 100; ++i) {
+        readPulseWidth += pulseIn(pin2, HIGH);
+        if (millis() - start > 3000) {
+            break; // pulseIn() takes too long time
+        }
+    }
+    readPulseWidth /= 100; // Average pulse width
+    servo2.detach();
+    assertTrue(readPulseWidth > pulseWidth - 50 && readPulseWidth < pulseWidth + 50);
+}