diff options
| -rw-r--r-- | src/stepper.c | 26 |
1 files changed, 15 insertions, 11 deletions
diff --git a/src/stepper.c b/src/stepper.c index 6ce847f1..9dedaf45 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -32,7 +32,7 @@ struct stepper { }; enum { MF_DIR=1 }; -enum { SF_LAST_DIR=1, SF_NEXT_DIR=2, SF_INVERT_STEP=4 }; +enum { SF_LAST_DIR=1, SF_NEXT_DIR=2, SF_INVERT_STEP=4, SF_HAVE_ADD=8 }; // Setup a stepper for the next move in its queue static uint8_t @@ -46,10 +46,10 @@ stepper_load_next(struct stepper *s) return SF_DONE; } - s->interval = m->interval; - s->time.waketime += s->interval; + s->time.waketime += m->interval; s->add = m->add; - s->interval += s->add; + s->interval = m->interval + m->add; + s->flags = m->add ? s->flags | SF_HAVE_ADD : s->flags & ~SF_HAVE_ADD; s->count = m->count; if (m->flags & MF_DIR) { s->position = -s->position + s->count; @@ -73,8 +73,9 @@ stepper_event(struct timer *t) if (likely(count)) { s->count = count; s->time.waketime += s->interval; - s->interval += s->add; gpio_out_toggle(s->step_pin); + if (s->flags & SF_HAVE_ADD) + s->interval += s->add; return SF_RESCHEDULE; } uint8_t ret = stepper_load_next(s); @@ -104,19 +105,19 @@ command_queue_step(uint32_t *args) { struct stepper *s = lookup_oid(args[0], command_config_stepper); struct move *m = move_alloc(); - m->flags = 0; - if (!!(s->flags & SF_LAST_DIR) != !!(s->flags & SF_NEXT_DIR)) { - s->flags ^= SF_LAST_DIR; - m->flags |= MF_DIR; - } m->interval = args[1]; m->count = args[2]; if (!m->count) shutdown("Invalid count parameter"); m->add = args[3]; m->next = NULL; + m->flags = 0; uint8_t flag = irq_save(); + if (!!(s->flags & SF_LAST_DIR) != !!(s->flags & SF_NEXT_DIR)) { + s->flags ^= SF_LAST_DIR; + m->flags |= MF_DIR; + } if (s->count) { if (s->first) *s->plast = m; @@ -138,7 +139,10 @@ void command_set_next_step_dir(uint32_t *args) { struct stepper *s = lookup_oid(args[0], command_config_stepper); - s->flags = (s->flags & ~SF_NEXT_DIR) | (args[1] ? SF_NEXT_DIR : 0); + uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0; + irq_disable(); + s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir; + irq_enable(); } DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c"); |