From 57fc14fd4a9d3ddb4db39e558f7f2031bdc8a923 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 19 Nov 2019 16:05:52 -0800 Subject: [PATCH] Use util/atomic.h, Small stepper interrupt speed improvement, Code cleanup, Pass structs by ref, Fixed slave motor homing synch problem. --- src/avr/src/axis.c | 22 ++--------- src/avr/src/config.h | 20 +++++----- src/avr/src/drv8711.c | 2 +- src/avr/src/exec.c | 5 ++- src/avr/src/huanyang.c | 13 ++++--- src/avr/src/motor.c | 80 +++++++++++++++++++-------------------- src/avr/src/pwm.c | 2 +- src/avr/src/pwm.h | 2 +- src/avr/src/ringbuf.def | 10 ++--- src/avr/src/spindle.c | 2 +- src/avr/src/spindle.h | 2 +- src/avr/src/stepper.c | 39 +++++++++++-------- src/avr/src/usart.c | 8 ++-- src/avr/src/vfd_spindle.c | 13 ++++--- 14 files changed, 104 insertions(+), 116 deletions(-) diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index d85c002..1f891f5 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -95,23 +95,7 @@ void axis_map_motors() { void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;} -#define AXIS_GET(NAME, TYPE, DEFAULT) \ - TYPE axis_get_##NAME(int axis) { \ - int motor = axis_get_motor(axis); \ - return motor == -1 ? DEFAULT : axes[motor].NAME; \ - } \ - AXIS_VAR_GET(NAME, TYPE) - - -#define AXIS_SET(NAME, TYPE) \ - void axis_set_##NAME(int axis, TYPE value) { \ - int motor = axis_get_motor(axis); \ - if (motor != -1) axes[motor].NAME = value; \ - } \ - AXIS_VAR_SET(NAME, TYPE) - - -/// Velocity is scaled by 1,000. +/// Velocity is scaled by 1,000 float axis_get_velocity_max(int axis) { int motor = axis_get_motor(axis); return motor == -1 ? 0 : axes[motor].velocity_max * VELOCITY_MULTIPLIER; @@ -119,7 +103,7 @@ float axis_get_velocity_max(int axis) { AXIS_VAR_GET(velocity_max, float) -/// Acceleration is scaled by 1,000. +/// Acceleration is scaled by 1,000 float axis_get_accel_max(int axis) { int motor = axis_get_motor(axis); return motor == -1 ? 0 : axes[motor].accel_max * ACCEL_MULTIPLIER; @@ -127,7 +111,7 @@ float axis_get_accel_max(int axis) { AXIS_VAR_GET(accel_max, float) -/// Jerk is scaled by 1,000,000. +/// Jerk is scaled by 1,000,000 float axis_get_jerk_max(int axis) { int motor = axis_get_motor(axis); return motor == -1 ? 0 : axes[motor].jerk_max * JERK_MULTIPLIER; diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 68df384..a224b4d 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -112,12 +112,15 @@ enum { /* Interrupt usage: * - * HI Stepper timers (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED Serial RX (set in usart.c) - * MED Serial TX (set in usart.c) (* see note) - * MED I2C Slave (set in i2c.c) - * LO Real time clock interrupt (set in rtc.h) + * HI Step timers stepper.c + * HI Serial RX usart.c + * MED Serial TX usart.c (* see note) + * MED Modbus serial interrupts modbus.c + * LO Segment execution SW interrupt stepper.c + * LO I2C Slave i2c.c + * LO Real-time clock interrupt rtc.c + * LO DRV8711 SPI drv8711.c + * LO A2D interrupts analog.c * * (*) The TX cannot run at LO level or exception reports and other prints * called from a LO interrupt (as in prep_line()) will kill the system @@ -125,19 +128,16 @@ enum { */ // Timer assignments -// NOTE, TCC1 free +// NOTE, TCC1 is unused #define TIMER_STEP TCC0 // Step timer (see stepper.h) #define TIMER_PWM TCD1 // PWM timer (see pwm.c) // Timer setup for stepper and dwells -#define STEP_TIMER_ENABLE TC_CLKSEL_DIV8_gc #define STEP_TIMER_DIV 8 #define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) #define STEP_TIMER_POLL ((uint16_t)(STEP_TIMER_FREQ * 0.001)) // 1ms -#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover #define STEP_TIMER_ISR TCC0_OVF_vect -#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc #define STEP_LOW_LEVEL_ISR ADCB_CH0_vect #define STEP_PULSE_WIDTH (F_CPU * 0.000002 / 2) // 2uS w/ clk/2 #define SEGMENT_MS 4 diff --git a/src/avr/src/drv8711.c b/src/avr/src/drv8711.c index ca7a8f6..dbc4de3 100644 --- a/src/avr/src/drv8711.c +++ b/src/avr/src/drv8711.c @@ -322,7 +322,7 @@ static void _motor_fault_switch_cb(switch_id_t sw, bool active) { void drv8711_init() { // Setup pins - // Must set the SS pin either in/high or any/output for master mode to work + // Must set the SS pin either in/high or out/any for master mode to work // Note, this pin is also used by the USART as the CTS line DIRSET_PIN(SPI_SS_PIN); // Output OUTSET_PIN(SPI_CLK_PIN); // High diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index c8f9314..e5f6fe0 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -300,6 +300,7 @@ void command_set_axis_exec(void *data) { ex.position[cmd->axis] = cmd->position; // Update motors - int motor = axis_get_motor(cmd->axis); - if (0 <= motor) motor_set_position(motor, cmd->position); + for (int motor = 0; motor < MOTORS; motor++) + if (motor_get_axis(motor) == cmd->axis) + motor_set_position(motor, cmd->position); } diff --git a/src/avr/src/huanyang.c b/src/avr/src/huanyang.c index 8986702..26481e1 100644 --- a/src/avr/src/huanyang.c +++ b/src/avr/src/huanyang.c @@ -30,6 +30,8 @@ #include "modbus.h" #include "estop.h" +#include + #include #include @@ -279,12 +281,11 @@ void huanyang_deinit(deinit_cb_t cb) { void huanyang_set(float power) { - cli(); - if (hy.power != power && !hy.shutdown) { - hy.power = power; - hy.changed = true; - } - sei(); + if (hy.power != power && !hy.shutdown) + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + hy.power = power; + hy.changed = true; + } } diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index 2edc42c..cee1e9c 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -224,59 +224,59 @@ void motor_emulate_steps(int motor) { void motor_end_move(int motor) { - motor_t *m = &motors[motor]; + motor_t &m = motors[motor]; - if (!m->timer->CTRLA) return; + if (!m.timer->CTRLA) return; // Stop clock - m->timer->CTRLA = 0; + m.timer->CTRLA = 0; // Wait for pending DMA transfers - while (m->dma->CTRLB & DMA_CH_CHPEND_bm) continue; + while (m.dma->CTRLB & DMA_CH_CHPEND_bm) continue; // Get actual step count from DMA channel - const int32_t steps = 0xffff - m->dma->TRFCNT; + const int32_t steps = 0xffff - m.dma->TRFCNT; // Accumulate encoder & compute error - m->encoder += m->last_negative ? -steps : steps; - m->error = m->commanded - m->encoder; + m.encoder += m.last_negative ? -steps : steps; + m.error = m.commanded - m.encoder; } void motor_load_move(int motor) { - motor_t *m = &motors[motor]; + motor_t &m = motors[motor]; // Clear move - ESTOP_ASSERT(m->prepped, STAT_MOTOR_NOT_PREPPED); - m->prepped = false; + ESTOP_ASSERT(m.prepped, STAT_MOTOR_NOT_PREPPED); + m.prepped = false; motor_end_move(motor); - if (!m->timer_period) return; // Leave clock stopped + if (!m.timer_period) return; // Leave clock stopped // Set direction, compensating for polarity but only when moving - const bool dir = m->negative ^ m->reverse; - if (dir != IN_PIN(m->dir_pin)) { - SET_PIN(m->dir_pin, dir); + const bool dir = m.negative ^ m.reverse; + if (dir != IN_PIN(m.dir_pin)) { + SET_PIN(m.dir_pin, dir); // We need at least 200ns between direction change and next step. - if (m->timer->CCA < m->timer->CNT) m->timer->CNT = m->timer->CCA + 1; + if (m.timer->CCA < m.timer->CNT) m.timer->CNT = m.timer->CCA + 1; } // Reset DMA step counter - m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; - m->dma->TRFCNT = 0xffff; - m->dma->CTRLA |= DMA_CH_ENABLE_bm; + m.dma->CTRLA &= ~DMA_CH_ENABLE_bm; + m.dma->TRFCNT = 0xffff; + m.dma->CTRLA |= DMA_CH_ENABLE_bm; // To avoid causing couter wrap around, it is important to start the clock // before setting PERBUF. If PERBUF is set before the clock is started PER // updates immediately and possibly mid step. // Set clock and period - m->timer->CTRLA = m->clock; // Start clock - m->timer->PERBUF = m->timer_period; // Set next frequency - m->last_negative = m->negative; - m->commanded = m->position; + m.timer->CTRLA = m.clock; // Start clock + m.timer->PERBUF = m.timer_period; // Set next frequency + m.last_negative = m.negative; + m.commanded = m.position; } @@ -285,27 +285,27 @@ void motor_prep_move(int motor, float target) { ESTOP_ASSERT(0 <= motor && motor < MOTORS, STAT_MOTOR_ID_INVALID); ESTOP_ASSERT(isfinite(target), STAT_BAD_FLOAT); - motor_t *m = &motors[motor]; - ESTOP_ASSERT(!m->prepped, STAT_MOTOR_NOT_READY); + motor_t &m = motors[motor]; + ESTOP_ASSERT(!m.prepped, STAT_MOTOR_NOT_READY); // Travel in steps int32_t position = _position_to_steps(motor, target); - int24_t steps = position - m->position; - m->position = position; + int24_t steps = position - m.position; + m.position = position; // Error correction - int16_t correction = abs(m->error); + int16_t correction = abs(m.error); if (MIN_STEP_CORRECTION <= correction) { // Dampen correction oscillation correction >>= 1; // Make correction - steps += m->error < 0 ? -correction : correction; + steps += m.error < 0 ? -correction : correction; } // Positive steps from here on - m->negative = steps < 0; - if (m->negative) steps = -steps; + m.negative = steps < 0; + if (m.negative) steps = -steps; // Start with clock / 2 const float seg_clocks = SEGMENT_TIME * (F_CPU * 60 / 2); @@ -321,27 +321,27 @@ void motor_prep_move(int motor, float target) { // Use faster clock with faster step rates for increased resolution. if (ticks_per_step < 0x7fff) { ticks_per_step *= 2; - m->clock = TC_CLKSEL_DIV1_gc; + m.clock = TC_CLKSEL_DIV1_gc; - } else m->clock = TC_CLKSEL_DIV2_gc; + } else m.clock = TC_CLKSEL_DIV2_gc; // Disable clock if too slow if (0xffff <= ticks_per_step) ticks_per_step = 0; - m->timer_period = steps ? round(ticks_per_step) : 0; + m.timer_period = steps ? round(ticks_per_step) : 0; // Power motor - if (!m->enabled) { - m->timer_period = 0; - m->encoder = m->commanded = m->position; - m->error = 0; + if (!m.enabled) { + m.timer_period = 0; + m.encoder = m.commanded = m.position; + m.error = 0; - } else if (m->timer_period) // Motor is moving so reset power timeout - m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; + } else if (m.timer_period) // Motor is moving so reset power timeout + m.power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; _update_power(motor); // Queue move - m->prepped = true; + m.prepped = true; } diff --git a/src/avr/src/pwm.c b/src/avr/src/pwm.c index 524a4a3..1659964 100644 --- a/src/avr/src/pwm.c +++ b/src/avr/src/pwm.c @@ -173,7 +173,7 @@ power_update_t pwm_get_update(float power) { // Called from hi-priority stepper interrupt, must be very fast -void pwm_update(power_update_t update) { +void pwm_update(const power_update_t &update) { if (!pwm.initialized || update.state == POWER_IGNORE) return; _update_clock(update.period); if (update.period) _set_dir(update.state == POWER_FORWARD); diff --git a/src/avr/src/pwm.h b/src/avr/src/pwm.h index 044d811..23c723b 100644 --- a/src/avr/src/pwm.h +++ b/src/avr/src/pwm.h @@ -34,4 +34,4 @@ void pwm_init(); float pwm_get(); void pwm_deinit(deinit_cb_t cb); power_update_t pwm_get_update(float power); -void pwm_update(power_update_t update); +void pwm_update(const power_update_t &update); diff --git a/src/avr/src/ringbuf.def b/src/avr/src/ringbuf.def index 9fff964..1f4bad9 100644 --- a/src/avr/src/ringbuf.def +++ b/src/avr/src/ringbuf.def @@ -122,17 +122,13 @@ RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() { } -#define _RING_BUF_ATOMIC_WRITE_INDEX(INDEX, TARGET) \ +#define RING_BUF_ATOMIC_WRITE_INDEX(INDEX) \ RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _write_##INDEX) \ - (RING_BUF_INDEX_TYPE value) { \ - RING_BUF_ATOMIC_COPY(TARGET, value); \ + (RING_BUF_INDEX_TYPE value) { \ + RING_BUF_ATOMIC_COPY(RING_BUF.INDEX, value); \ } -#define RING_BUF_ATOMIC_WRITE_INDEX(INDEX) \ - _RING_BUF_ATOMIC_WRITE_INDEX(INDEX, RING_BUF.INDEX) - - RING_BUF_ATOMIC_READ_INDEX(head); RING_BUF_ATOMIC_READ_INDEX(tail); RING_BUF_ATOMIC_WRITE_INDEX(head); diff --git a/src/avr/src/spindle.c b/src/avr/src/spindle.c index 6b2566d..cd39a88 100644 --- a/src/avr/src/spindle.c +++ b/src/avr/src/spindle.c @@ -189,7 +189,7 @@ void spindle_load_power_updates(power_update_t updates[], float minD, // Called from hi-priority stepper interrupt -void spindle_update(power_update_t update) {pwm_update(update);} +void spindle_update(const power_update_t &update) {pwm_update(update);} void spindle_update_speed() {_set_speed(spindle.speed);} diff --git a/src/avr/src/spindle.h b/src/avr/src/spindle.h index b3f8395..556a512 100644 --- a/src/avr/src/spindle.h +++ b/src/avr/src/spindle.h @@ -67,6 +67,6 @@ void spindle_stop(); void spindle_estop(); void spindle_load_power_updates(power_update_t updates[], float minD, float maxD); -void spindle_update(power_update_t update); +void spindle_update(const power_update_t &update); void spindle_update_speed(); void spindle_idle(); diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index 21feec0..68453f7 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -36,6 +36,8 @@ #include "exec.h" #include "drv8711.h" +#include + #include #include @@ -45,14 +47,16 @@ typedef struct { bool busy; bool requesting; float dwell; + uint8_t power_buf; uint8_t power_index; - power_update_t powers[POWER_MAX_UPDATES]; // Move prep bool move_ready; // Prepped move ready for loader bool move_queued; // Prepped move queued float prep_dwell; - power_update_t prep_powers[POWER_MAX_UPDATES]; + int8_t power_next; + + power_update_t powers[2][POWER_MAX_UPDATES]; uint32_t underrun; } stepper_t; @@ -63,10 +67,10 @@ static stepper_t st = {0}; void stepper_init() { // Setup step timer - TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // Waveform mode - TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // Interrupt mode - TIMER_STEP.PER = STEP_TIMER_POLL; // Timer rate - TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // Start step timer + TIMER_STEP.CTRLB = TC_WGMODE_NORMAL_gc; // Count to TOP & rollover + TIMER_STEP.INTCTRLA = TC_OVFINTLVL_HI_gc; // Interrupt level + TIMER_STEP.PER = STEP_TIMER_POLL; // Timer rate + TIMER_STEP.CTRLA = TC_CLKSEL_DIV8_gc; // Start step timer } @@ -141,7 +145,7 @@ static void _request_exec_move() { static void _update_power() { if (st.power_index < POWER_MAX_UPDATES) - spindle_update(st.powers[st.power_index++]); + spindle_update(st.powers[st.power_buf][st.power_index++]); } @@ -189,10 +193,12 @@ ISR(STEP_TIMER_ISR) { } // Handle power updates - st.power_index = 0; - memcpy(st.powers, st.prep_powers, sizeof(st.powers)); - memset(st.prep_powers, 0, sizeof(st.prep_powers)); - _update_power(); + if (st.power_next != -1) { + st.power_index = 0; + st.power_buf = st.power_next; + st.power_next = -1; + _update_power(); + } st.busy = true; // Executing move so mark busy st.move_ready = false; // We are done with this move, flip the flag back @@ -201,7 +207,9 @@ ISR(STEP_TIMER_ISR) { void st_prep_power(const power_update_t powers[]) { ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY); - memcpy(st.prep_powers, powers, sizeof(st.prep_powers)); + st.power_next = !st.power_buf; + memcpy(st.powers[st.power_next], powers, + sizeof(power_update_t) * POWER_MAX_UPDATES); } @@ -221,7 +229,8 @@ void st_prep_line(const float target[]) { void st_prep_dwell(float seconds) { ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY); if (seconds <= 1e-4) seconds = 1e-4; // Min dwell - spindle_load_power_updates(st.prep_powers, 0, 0); + st.power_next = !st.power_buf; + spindle_load_power_updates(st.powers[st.power_next], 0, 0); st.prep_dwell = seconds; st.move_queued = true; // signal prep buffer ready } @@ -233,8 +242,6 @@ uint32_t get_underrun() {return st.underrun;} float get_dwell_time() { float dwell; - cli(); - dwell = st.dwell; - sei(); + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) dwell = st.dwell; return dwell; } diff --git a/src/avr/src/usart.c b/src/avr/src/usart.c index f364332..d035c0e 100644 --- a/src/avr/src/usart.c +++ b/src/avr/src/usart.c @@ -84,12 +84,10 @@ ISR(SERIAL_DRE_vect) { // Data received interrupt vector ISR(SERIAL_RXC_vect) { if (rx_buf_full()) _set_rxc_interrupt(false); // Disable interrupt + else rx_buf_push(SERIAL_PORT.DATA); - else { - rx_buf_push(SERIAL_PORT.DATA); - if (rx_buf_space() < SERIAL_CTS_THRESH) - OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable) - } + if (rx_buf_space() < SERIAL_CTS_THRESH) + OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable) } diff --git a/src/avr/src/vfd_spindle.c b/src/avr/src/vfd_spindle.c index 7c9530c..40cdf4a 100644 --- a/src/avr/src/vfd_spindle.c +++ b/src/avr/src/vfd_spindle.c @@ -32,6 +32,8 @@ #include "estop.h" #include "pgmspace.h" +#include + #include #include #include @@ -383,12 +385,11 @@ void vfd_spindle_deinit(deinit_cb_t cb) { void vfd_spindle_set(float power) { - cli(); - if (vfd.power != power) { - vfd.power = power; - vfd.changed = true; - } - sei(); + if (vfd.power != power) + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + vfd.power = power; + vfd.changed = true; + } } -- 2.27.0