diff --git a/include/gpio.h b/include/gpio.h
index fc8bb16776a1775cfa2581e50e2b25785dedc579..902deed83c2b080d7ddbf6b36d6c79f7a48717fa 100755
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -7,6 +7,8 @@ extern "C" {
 
 #include "stm32f1xx.h"
 
+#define           GPIO_RGB(R,G,B)         ((R&0x1F)+((G&0x1F)<<5)+((B&0x1F)<<10))
+
 #define           GPIO_NUM_LEDS           11
 
 typedef enum {LED_TX=0,LED_RX=1,LED_2=2,LED_3=3,LED_4=4,LED_5_R=5,LED_5_G=6,LED_5_B=7,LED_6_R=8,LED_6_G=9,LED_6_B=10} led_t;
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 7f514c92ad653640d71bd5dd1b4547271ff629e0..3c701a1d709ef38c08fde876cde5f23baf105148 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -118,7 +118,7 @@ void darwin_dyn_slave_init(void)
   priorities.dma_tx_subpriority=0;
 
   usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities);
-  dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,0x01,DYN_VER2);
+  dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,ram_data[DEVICE_ID_OFFSET],DYN_VER2);
   darwin_dyn_slave.on_read=darwin_on_read;
   darwin_dyn_slave.on_write=darwin_on_write;
   darwin_dyn_slave.on_ping=darwin_on_ping;
diff --git a/src/gpio.c b/src/gpio.c
index d6a64f810b487235c3478e6d70989952223d8cf4..bffb9acb66e32c002b056dc944857bc8bbdfb662 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -566,6 +566,7 @@ void gpio_toggle_led(led_t led_id)
 void gpio_blink_led(led_t led_id, uint16_t period_ms)
 {
   TIM_OC_InitTypeDef TIM_OCInitStructure;
+  uint16_t capture;
 
   if(led_id>=LED_TX && led_id<=LED_3)
   {
@@ -575,7 +576,8 @@ void gpio_blink_led(led_t led_id, uint16_t period_ms)
     if(period_ms>1)
     {
       leds_data[led_id]=period_ms;
-      TIM_OCInitStructure.Pulse = period_ms;
+      capture = HAL_TIM_ReadCapturedValue(leds[led_id].timer,leds[led_id].timer_ch);
+      TIM_OCInitStructure.Pulse = capture + period_ms;
       HAL_TIM_OC_ConfigChannel(leds[led_id].timer, &TIM_OCInitStructure,leds[led_id].timer_ch);
       HAL_TIM_OC_Start_IT(leds[led_id].timer,leds[led_id].timer_ch);
     }
@@ -587,26 +589,29 @@ void gpio_blink_led(led_t led_id, uint16_t period_ms)
 void gpio_set_color(led_t led_id, uint16_t value)
 {
   TIM_OC_InitTypeDef TIM_OCInitStructure;
+  uint16_t capture;
 
   if(led_id>=LED_5_R && led_id<=LED_6_B)
   {
     TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
     TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
-    if(value==0)
+
+    if(led_id==LED_5_R || led_id==LED_6_R)
+      leds_data[led_id]=value&GPIO_RED_COLOR;
+    else if(led_id==LED_5_G || led_id==LED_6_G)
+      leds_data[led_id]=((value&GPIO_GREEN_COLOR)>>5);
+    else 
+      leds_data[led_id]=((value&GPIO_BLUE_COLOR)>>10);
+    if(leds_data[led_id]==0)
     {
       HAL_TIM_OC_Stop_IT(leds[led_id].timer,leds[led_id].timer_ch);
       gpio_clear_led(led_id);
     }
-    else if(value<31)
+    else if(leds_data[led_id]<31)
     {
-      if(led_id==LED_5_R || led_id==LED_6_R)
-        leds_data[led_id]=value&GPIO_RED_COLOR;
-      else if(led_id==LED_5_G || led_id==LED_6_G)
-        leds_data[led_id]=((value&GPIO_GREEN_COLOR)>>5);
-      else 
-        leds_data[led_id]=((value&GPIO_BLUE_COLOR)>>10);
-      TIM_OCInitStructure.Pulse = leds_data[led_id];
+      capture = HAL_TIM_ReadCapturedValue(leds[led_id].timer,leds[led_id].timer_ch);
+      TIM_OCInitStructure.Pulse = capture + leds_data[led_id];
       HAL_TIM_OC_ConfigChannel(leds[led_id].timer,&TIM_OCInitStructure,leds[led_id].timer_ch);
       HAL_TIM_OC_Start_IT(leds[led_id].timer,leds[led_id].timer_ch);
     }