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.