From d4fed6cfe81b2cb6ea42cf5d765c5f129f6494a5 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sun, 17 Mar 2024 22:04:52 +0100 Subject: [PATCH] Yes it works now. Made another implementation of the pulse IRQ and all that. We'll see if I keep this or go back to the older. It is IMPORTANT, REQUIRED to use a 4.9 linux kernel for it to work. There are obviously bugs in the RealTek network drivers R8168/R8169 in 5+ kernels. All this work could have been avoided with a 4.9 kernel. --- Firmware/include/StepGen2.h | 11 ++++++----- Firmware/src/StepGen2.cpp | 34 ++++++++++++++++++++++++---------- Firmware/src/main.cpp | 19 ++++++++++--------- 3 files changed, 40 insertions(+), 24 deletions(-) diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h index 777b0dd..ccfadf0 100755 --- a/Firmware/include/StepGen2.h +++ b/Firmware/include/StepGen2.h @@ -8,8 +8,9 @@ class StepGen2 public: volatile double_t actualPosition; volatile int32_t nSteps; - volatile uint32_t timerPulseSteps; volatile uint32_t timerFrequency; + volatile int32_t timerPosition = 0; + volatile int32_t timerEndPosition = 0; public: volatile float Tstartf; // Starting delay in secs @@ -21,7 +22,7 @@ public: HardwareTimer *startTimer; // Use timers 10,11,13,14 uint8_t dirPin; PinName stepPin; - uint32_t Tjitter = 400; // Longest time from IRQ to handling in handleStepper, unit is microseconds + uint32_t Tjitter = 350; // Longest time from IRQ to handling in handleStepper, unit is microseconds uint64_t dbg; const uint16_t t2 = 5; // DIR is ahead of PUL with at least 5 usecs const uint16_t t3 = 3; // Pulse width at least 2.5 usecs @@ -37,8 +38,8 @@ public: volatile uint8_t enabled; // Enabled step generator volatile float frequency; - static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds - volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often) + static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds + volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often) StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void)); @@ -52,7 +53,7 @@ class extend32to64 { public: int64_t previousTimeValue = 0; - const uint64_t ONE_PERIOD = 4294967296; // almost UINT32_MAX; + const uint64_t ONE_PERIOD = 4294967296; // almost UINT32_MAX; const uint64_t HALF_PERIOD = 2147483648; // Half of that int64_t extendTime(uint32_t in); }; diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp index 7dc42b9..ba30e05 100755 --- a/Firmware/src/StepGen2.cpp +++ b/Firmware/src/StepGen2.cpp @@ -63,11 +63,14 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops) Tpulses = abs(nSteps) / frequency; } updatePos(5); - return 1; + uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs) dbg = timeSinceISR; // Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq. + if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging + return updatePos(1); + timerFrequency = uint32_t(ceil(frequency)); startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs startTimer->refresh(); @@ -77,13 +80,13 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops) void StepGen2::startTimerCB() { - startTimer->pause(); // Once is enough. - digitalWrite(dirPin, nSteps < 0 ? HIGH : LOW); - // There will be a short break here for t2 usecs, in the future. - timerPulseSteps = abs(nSteps); - pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin); + startTimer->pause(); // Once is enough. + digitalWriteFast(digitalPinToPinName(dirPin), nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH + // There will be a short break here for t2 usecs, in the future. + timerEndPosition += nSteps; + pulseTimer->pause(); + pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM1, stepPin); pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT); - // pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT); pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT); pulseTimer->refresh(); pulseTimer->resume(); @@ -91,10 +94,21 @@ void StepGen2::startTimerCB() void StepGen2::pulseTimerCB() { - --timerPulseSteps; - if (timerPulseSteps == 0) - { + int16_t dir = digitalReadFast(digitalPinToPinName(dirPin)); + if (dir == HIGH) + timerPosition--; + else + timerPosition++; + int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps + if (diffPosition == 0) pulseTimer->pause(); + else + { + if (diffPosition < 0 && dir == LOW) // Change direction. Should not end up here, but alas + digitalWriteFast(digitalPinToPinName(dirPin), HIGH); // Normal is to be HIGH when decreasing + if (diffPosition > 0 && dir == HIGH) // Change direction. Should not end up here, but alas + digitalWriteFast(digitalPinToPinName(dirPin), LOW); // Normal is to be LOW when increasing + // Normally nothing is needed } } diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 18e7109..35ed3a0 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -45,8 +45,6 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera // Step2.reqPos(Obj.CommandedPosition2); // Step2.setScale(Obj.StepsPerMM2); // Step2.enable(1); - Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos(); - Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos(); } uint16_t nLoops; @@ -78,6 +76,9 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation Obj.EncPos = Encoder1.currentPos(); Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); Obj.IndexByte = Encoder1.getIndexState(); + float_t ap2 = Obj.ActualPosition2; + Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos(); + Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos(); uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds Tim.push(dTim); @@ -93,9 +94,9 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation thenTime = irqTime; Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug Obj.D1 = Step2.frequency; - Obj.D2 = nLoops; - Obj.D3 = max_Tim - min_Tim; - Obj.D4 = ALEventIRQ; + Obj.D2 = Step2.nSteps; + Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos(); + Obj.D4 = Step2.Tstartu; } void ESC_interrupt_enable(uint32_t mask); @@ -139,8 +140,8 @@ void loop(void) { nowTime = longTime.extendTime(micros()); /* Read local time from ESC*/ - ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time)); - ESCvar.Time = etohl(ESCvar.Time); + // ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time)); + // ESCvar.Time = etohl(ESCvar.Time); DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); serveIRQ = 0; @@ -148,14 +149,14 @@ void loop(void) ecat_slv_poll(); } dTime = longTime.extendTime(micros()) - irqTime; - if ((dTime > 500 && dTime < 800) || dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt + if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt ecat_slv_poll(); } void sync0Handler(void) { ALEventIRQ = ESC_ALeventread(); - if (ALEventIRQ & ESCREG_ALEVENT_SM2) + // if (ALEventIRQ & ESCREG_ALEVENT_SM2) { serveIRQ = 1; irqTime = longTime.extendTime(micros());