Clear ALevents for DC_sync0 and SM3 might have solved the uneven pulse train. Looking better now.

Firmware/include/StepGen2.h

@@ -9,7 +9,12 @@ private:
     volatile double_t actualPosition;
     volatile int32_t nSteps;
     volatile uint32_t timerPulseSteps;
-    volatile float Tstart;
+
+public:
+    volatile float Tstartf;    // Starting delay in secs
+    volatile uint32_t Tstartu; // Starting delay in usecs
+private:
+public:
     const float maxAllowedFrequency = 100000; // 100 kHz for now
     HardwareTimer *pulseTimer;
     uint32_t pulseTimerChan;
@@ -28,7 +33,7 @@ public:
     volatile float frequency;
 
     static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time
-    volatile uint32_t lcncCycleTime; // Linuxcnc nominal cycle time (1 ms often)
+    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));
 

Firmware/src/StepGen2.cpp

@@ -21,49 +21,52 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
     startTimer = new HardwareTimer(Timer2);
     startTimer->attachInterrupt(irq2);
 }
-
+uint32_t cnt = 0;
 uint32_t StepGen2::handleStepper(void)
 {
+
     if (!enabled)
         return updatePos(0);
-    lcncCycleTime = StepGen2::sync0CycleTime;
+
+    lcncCycleTime = StepGen2::sync0CycleTime * 1.0e-6;             // was usec, became sec
     commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
     if (initialStepPosition == commandedStepPosition)              // No movement
-        return updatePos(1);
-
-    float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step
-                                 / (float)lcncCycleTime;                           // from here on
-    if (approximateFrequency > maxAllowedFrequency)                                // Stay on this position
         return 1;
+//    digitalWrite(dirPin, cnt++ % 2);
+    float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step
+                                 / lcncCycleTime;                                  // from here on
+ //   if (approximateFrequency > maxAllowedFrequency)                                // Stay on this position
+ //       return 1;
 
-    float kTRAJ = (commandedPosition - initialPosition) / float(lcncCycleTime); // Straight line equation
+    float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation
     float mTRAJ = initialPosition;                                       // position = kTRAJ x time + mTRAJ
                                                                          // Operating on incoming positions (not steps)
     if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01)                 // Very flat slope
     {                                                                    //
-        Tstart = 0.5 * lcncCycleTime;                                           // Just take a step in the middle of the cycle
+        Tstartf = 0.5 * lcncCycleTime;                                   // Just take a step in the middle of the cycle
         frequency = 10000;                                               // At some suitable frequency
         nSteps = kTRAJ > 0 ? 1 : -1;                                     // Take only one step
     }
     else // Regular step train, up or down
     {
         if (kTRAJ > 0)
-            Tstart = (ceil(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
+            Tstartf = (ceil(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ;
         else
-            Tstart = (floor(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
+            Tstartf = (floor(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ;
-        frequency = kTRAJ * stepsPerMM;
+        frequency = fabs(kTRAJ * stepsPerMM);
         nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction.
     }
     updatePos(5);
+    Tstartu = Tstartf * 1e6; // Was secs, now usecs
 
-    startTimer->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs
+    startTimer->setOverflow(Tstartu + Tjitter, MICROSEC_FORMAT); // All handled by irqs
     startTimer->resume();
     return 1;
 }
 void StepGen2::startTimerCB()
 {
     startTimer->pause(); // Once is enough.
-    digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
+    // digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
     timerPulseSteps = abs(nSteps);
     pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
     pulseTimer->setOverflow(uint32_t(frequency), HERTZ_FORMAT);

Firmware/src/main.cpp

@@ -25,7 +25,8 @@ StepGen2 Step(TIM1, 4, PA_11, PA12, pulseTimerCallback, TIM10, startTimerCallbac
 void pulseTimerCallback(void) { Step.pulseTimerCB(); }
 void startTimerCallback(void) { Step.startTimerCB(); }
 CircularBuffer<uint32_t, 200> Tim;
-volatile uint64_t nowTime = 0, thenTime = 0;
+volatile uint64_t irqTime = 0, thenTime = 0;
+int64_t extendTime(uint32_t in); // Extend from 32-bit to 64-bit precision
 
 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
 {
@@ -38,8 +39,11 @@ void handleStepper(void)
    Step.enabled = true;
    Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
    Obj.StepGenOut1.ActualPosition = Step.commandedPosition;
-
+   Step.stepsPerMM = Obj.StepGenIn1.StepsPerMM;
+   Step.stepsPerMM = 4000;
    Step.handleStepper();
+
+   Obj.StepGenOut2.ActualPosition = Obj.StepGenIn2.CommandedPosition;
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -49,7 +53,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
    Obj.IndexByte = Encoder1.getIndexState();
 
-   uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
+   uint32_t dTim = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
    Tim.push(dTim);
    uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
    for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++)
@@ -60,8 +64,9 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
       if (aTim < min_Tim)
          min_Tim = aTim;
    }
-   thenTime = nowTime;
+   thenTime = irqTime;
    Obj.DiffT = max_Tim - min_Tim; // Debug
+   Obj.DiffT = ESCvar.ALevent;
 }
 
 void ESC_interrupt_enable(uint32_t mask);
@@ -100,22 +105,27 @@ void setup(void)
 
 void loop(void)
 {
+   uint32_t dTime;
    if (serveIRQ)
    {
-      nowTime = micros();
+      CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
       DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
                   DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
       serveIRQ = 0;
       ESCvar.PrevTime = ESCvar.Time;
    }
-   uint32_t dTime = micros() - nowTime;
+   dTime = micros() - irqTime;
    if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt
       ecat_slv_poll();
 }
-
+volatile uint32_t cmt;
 void sync0Handler(void)
 {
+   uint32_t lTime;
+   irqTime = micros();
    serveIRQ = 1;
+   ESC_read(ESCREG_LOCALTIME, (void *)&lTime, sizeof(lTime)); // Careful! Reads and writes update ALevent also.
+   digitalWrite(Step.dirPin, cmt++ % 2);
 }
 
 void ESC_interrupt_enable(uint32_t mask)
@@ -125,6 +135,8 @@ void ESC_interrupt_enable(uint32_t mask)
    uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2
    if (mask & user_int_mask)
    {
+      ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
+
       ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
       attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
 
@@ -165,3 +177,23 @@ uint16_t dc_checker(void)
    StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
    return 0;
 }
+
+#define ONE_PERIOD UINT32_MAX
+#define HALF_PERIOD (UINT32_MAX >> 1)
+static int64_t previousTimeValue = 0;
+
+int64_t extendTime(uint32_t in) // Extend from 32-bit to 64-bit precision
+{
+   int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
+   int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current
+
+   // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
+   int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
+   if (dif < -HALF_PERIOD)
+      mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
+
+   int64_t unwrapped = previousTimeValue + mod_dif;
+   previousTimeValue = unwrapped; // load previous value
+
+   return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
+}