diff options
| -rw-r--r-- | src/stepper.c | 51 |
1 files changed, 26 insertions, 25 deletions
diff --git a/src/stepper.c b/src/stepper.c index c1bdf275..749d470e 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -35,9 +35,10 @@ struct stepper { #if CONFIG_NO_UNSTEP_DELAY uint16_t count; #define next_step_time time.waketime +#define min_step_time time.waketime #else uint32_t count; - uint32_t next_step_time; + uint32_t next_step_time, min_step_time; #endif struct gpio_out step_pin, dir_pin; uint32_t position; @@ -54,7 +55,7 @@ enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_HAVE_ADD=1<<3 // Setup a stepper for the next move in its queue static uint_fast8_t -stepper_load_next(struct stepper *s, uint32_t min_next_time) +stepper_load_next(struct stepper *s) { struct stepper_move *m = s->first; if (!m) { @@ -66,6 +67,7 @@ stepper_load_next(struct stepper *s, uint32_t min_next_time) } s->next_step_time += m->interval; + s->time.waketime = s->next_step_time; s->add = m->add; s->interval = m->interval + m->add; if (CONFIG_NO_UNSTEP_DELAY) { @@ -74,17 +76,11 @@ stepper_load_next(struct stepper *s, uint32_t min_next_time) s->count = m->count; } else { // On faster mcus, it is necessary to schedule unstep events - // and so there are twice as many events. Also check that the - // next step event isn't too close to the last unstep. - if (unlikely(timer_is_before(s->next_step_time, min_next_time))) { - if ((int32_t)(s->next_step_time - min_next_time) - < (int32_t)(-timer_from_us(1000))) - shutdown("stepper too far in past"); - s->time.waketime = min_next_time; - } else { - s->time.waketime = s->next_step_time; - } + // and so there are twice as many events. s->count = m->count * 2; + if (timer_is_before(s->next_step_time + timer_from_us(1000) + , s->min_step_time)) + shutdown("stepper too far in past"); } if (m->flags & MF_DIR) { s->position = -s->position + m->count; @@ -118,30 +114,32 @@ stepper_event(struct timer *t) s->interval += s->add; return SF_RESCHEDULE; } - uint_fast8_t ret = stepper_load_next(s, 0); + uint_fast8_t ret = stepper_load_next(s); gpio_out_toggle(s->step_pin); return ret; } - // On faster mcus, it is necessary to schedule the unstep event - uint32_t min_next_time = timer_read_time() + UNSTEP_TIME; + // On faster mcus, it is necessary to wait between step and unstep + uint32_t curtime = timer_read_time(); + if (unlikely(timer_is_before(curtime, s->min_step_time))) + // Can't step/unstep yet - busy wait in scheduler + goto busy_wait; gpio_out_toggle(s->step_pin); + s->min_step_time = curtime + UNSTEP_TIME; s->count--; - if (likely(s->count & 1)) - // Schedule unstep event - goto reschedule_min; + if (s->count & 1) + // Started step - now busy wait in scheduler until unstep ready + goto busy_wait; if (likely(s->count)) { s->next_step_time += s->interval; s->interval += s->add; - if (unlikely(timer_is_before(s->next_step_time, min_next_time))) - // The next step event is too close - push it back - goto reschedule_min; s->time.waketime = s->next_step_time; return SF_RESCHEDULE; } - return stepper_load_next(s, min_next_time); -reschedule_min: - s->time.waketime = min_next_time; + return stepper_load_next(s); + +busy_wait: + s->time.waketime = curtime; return SF_RESCHEDULE; } @@ -202,7 +200,9 @@ command_queue_step(uint32_t *args) s->plast = &m->next; } else { s->first = m; - stepper_load_next(s, s->next_step_time + m->interval); + if (!CONFIG_NO_UNSTEP_DELAY) + s->min_step_time = s->next_step_time + m->interval; + stepper_load_next(s); sched_add_timer(&s->time); } irq_enable(); @@ -232,6 +232,7 @@ command_reset_step_clock(uint32_t *args) if (s->count) shutdown("Can't reset time when stepper active"); s->next_step_time = waketime; + s->min_step_time = s->next_step_time; s->flags |= SF_LAST_RESET; irq_enable(); } |