// Optimized step function for stepping and unstepping in same function
static uint_fast8_t
stepper_event_nodelay(struct stepper *s)
{
gpio_out_toggle_noirq(s->step_pin);
uint_fast16_t count = s->count - 1;
if (likely(count)) {
s->count = count;
s->time.waketime += s->interval;
s->interval += s->add;
gpio_out_toggle_noirq(s->step_pin);
return SF_RESCHEDULE;
}
uint_fast8_t ret = stepper_load_next(s, 0);
gpio_out_toggle_noirq(s->step_pin);
return ret;
}
// Timer callback - step the given stepper.
uint_fast8_t
stepper_event(struct timer *t)
{
struct stepper *s = container_of(t, struct stepper, time);
if (CONFIG_STEP_DELAY <= 0 && CONFIG_MACH_AVR)
return stepper_event_avr(s);
if (CONFIG_STEP_DELAY <= 0)
return stepper_event_nodelay(s);
// Normal step code - schedule the unstep event
uint32_t step_delay = timer_from_us(CONFIG_STEP_DELAY);
uint32_t min_next_time = timer_read_time() + step_delay;
gpio_out_toggle_noirq(s->step_pin);
s->count--;
if (likely(s->count & 1))
// Schedule unstep event
goto reschedule_min;
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 SF_RESCHEDULE;
}
void
gpio_out_toggle(struct gpio_out g)
{
irqstatus_t flag = irq_save();
gpio_out_toggle_noirq(g);
irq_restore(flag);
}
// 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)
{
struct stepper_move *m = s->first;
if (!m) {
if (s->interval - s->add < s->min_stop_interval
&& !(s->flags & SF_NO_NEXT_CHECK))
shutdown("No next step");
s->count = 0;
return SF_DONE;
}
s->next_step_time += m->interval;
s->add = m->add;
s->interval = m->interval + m->add;
if (CONFIG_STEP_DELAY <= 0) {
if (CONFIG_MACH_AVR)
// On AVR see if the add can be optimized away
s->flags = m->add ? s->flags|SF_HAVE_ADD : s->flags & ~SF_HAVE_ADD;
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;
}
s->count = (uint32_t)m->count * 2;
}
if (m->flags & MF_DIR) {
s->position = -s->position + m->count;
gpio_out_toggle_noirq(s->dir_pin);
} else {
s->position += m->count;
}
s->first = m->next;
move_free(m);
return SF_RESCHEDULE;
}
