diff options
| -rw-r--r-- | src/stepper.c | 44 |
1 files changed, 26 insertions, 18 deletions
diff --git a/src/stepper.c b/src/stepper.c index 95c9a2f2..7935c139 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -71,32 +71,40 @@ stepper_load_next(struct stepper *s) return SF_DONE; } - // Load next 'struct stepper_move' into 'struct stepper' + // Read next 'struct stepper_move' struct move_node *mn = move_queue_pop(&s->mq); struct stepper_move *m = container_of(mn, struct stepper_move, node); - s->add = m->add; - s->interval = m->interval + m->add; + uint32_t move_interval = m->interval; + uint_fast16_t move_count = m->count; + int_fast16_t move_add = m->add; + uint_fast8_t need_dir_change = m->flags & MF_DIR; + move_free(m); + + // Add all steps to s->position (stepper_get_position() can calc mid-move) + s->position = (need_dir_change ? -s->position : s->position) + move_count; + + // Load next move into 'struct stepper' + s->add = move_add; + s->interval = move_interval + move_add; if (HAVE_OPTIMIZED_PATH && s->flags & SF_OPTIMIZED_PATH) { - s->time.waketime += m->interval; + // Using optimized stepper_event_edge() or stepper_event_avr() + s->time.waketime += move_interval; if (HAVE_AVR_OPTIMIZATION) - s->flags = m->add ? s->flags|SF_HAVE_ADD : s->flags & ~SF_HAVE_ADD; - s->count = m->count; + s->flags = (move_add ? s->flags | SF_HAVE_ADD + : s->flags & ~SF_HAVE_ADD); + s->count = move_count; } else { - // It is necessary to schedule unstep events and so there are - // twice as many events. - s->next_step_time += m->interval; + // Using fully scheduled stepper_event_full() code (the scheduler + // may be called twice for each step) + s->next_step_time += move_interval; s->time.waketime = s->next_step_time; - s->count = s->flags & SF_SINGLE_SCHED ? m->count : (uint32_t)m->count*2; - } - // Add all steps to s->position (stepper_get_position() can calc mid-move) - if (m->flags & MF_DIR) { - s->position = -s->position + m->count; - gpio_out_toggle_noirq(s->dir_pin); - } else { - s->position += m->count; + s->count = (s->flags & SF_SINGLE_SCHED ? move_count + : (uint32_t)move_count * 2); } - move_free(m); + // Set new direction (if needed) + if (need_dir_change) + gpio_out_toggle_noirq(s->dir_pin); return SF_RESCHEDULE; } |