diff --git a/src/sched.c b/src/sched.c
index 51d159bc6..86f995fe3 100644
--- a/src/sched.c
+++ b/src/sched.c
@@ -86,12 +86,16 @@ void
 sched_add_timer(struct timer *add)
 {
     uint32_t waketime = add->waketime;
+    uint8_t err_flags = 0;
     irqstatus_t flag = irq_save();
     struct timer *tl = SchedStatus.timer_list;
     if (unlikely(timer_is_before(waketime, tl->waketime))) {
         // This timer is before all other scheduled timers
-        if (timer_is_before(waketime, timer_read_time()))
-            try_shutdown("Timer too close");
+        if (timer_is_before(waketime, timer_read_time())) {
+            err_flags = 1;
+            waketime = timer_read_time() + timer_from_us(2);
+            add->waketime = waketime;
+        }
         if (tl == &deleted_timer)
             add->next = deleted_timer.next;
         else
@@ -104,6 +108,9 @@ sched_add_timer(struct timer *add)
         insert_timer(tl, add, waketime);
     }
     irq_restore(flag);
+    if(err_flags) {			
+        output("Timer too close");
+    }
 }
 
 // The deleted timer is used when deleting an active timer.