Fixed bug in extend32to64:extendTime()

Firmware/include/StepGen2.h

@@ -52,8 +52,8 @@ 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;
+    const uint64_t HALF_PERIOD = 2147483648; // Half of that
     int64_t extendTime(uint32_t in);
 };
 

Firmware/lib/soes/ecat_slv.c

@@ -13,25 +13,25 @@
 #define IS_TXPDO(index) ((index) >= 0x1A00 && (index) < 0x1C00)
 
 /* Global variables used by the stack */
-uint8_t     MBX[MBXBUFFERS * MAX(MBXSIZE,MBXSIZEBOOT)];
+uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE, MBXSIZEBOOT)];
 _MBXcontrol MBXcontrol[MBXBUFFERS];
-_SMmap      SMmap2[MAX_MAPPINGS_SM2];
+_SMmap SMmap2[MAX_MAPPINGS_SM2];
-_SMmap      SMmap3[MAX_MAPPINGS_SM3];
+_SMmap SMmap3[MAX_MAPPINGS_SM3];
-_ESCvar     ESCvar;
+_ESCvar ESCvar;
 
 /* Private variables */
 static volatile int watchdog;
 
 #if MAX_MAPPINGS_SM2 > 0
-static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned (8)));
+static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned(8)));
 #else
-extern uint8_t * rxpdo;
+extern uint8_t *rxpdo;
 #endif
 
 #if MAX_MAPPINGS_SM3 > 0
-static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned (8)));
+static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned(8)));
 #else
-extern uint8_t * txpdo;
+extern uint8_t *txpdo;
 #endif
 
 /** Function to pre-qualify the incoming SDO download.
@@ -40,19 +40,19 @@ extern uint8_t * txpdo;
  * @param[in] sub-index  = sub-index of SDO download request to check
  * @return SDO abort code, or 0 on success
  */
-uint32_t ESC_download_pre_objecthandler (uint16_t index,
+uint32_t ESC_download_pre_objecthandler(uint16_t index,
-      uint8_t subindex,
+                                        uint8_t subindex,
-      void * data,
+                                        void *data,
-      size_t size,
+                                        size_t size,
-      uint16_t flags)
+                                        uint16_t flags)
 {
-   if (IS_RXPDO (index) ||
+   if (IS_RXPDO(index) ||
-       IS_TXPDO (index) ||
+       IS_TXPDO(index) ||
        index == RX_PDO_OBJIDX ||
        index == TX_PDO_OBJIDX)
    {
       uint8_t minSub = ((flags & COMPLETE_ACCESS_FLAG) == 0) ? 0 : 1;
-      if (subindex > minSub && COE_maxSub (index) != 0)
+      if (subindex > minSub && COE_maxSub(index) != 0)
       {
          return ABORT_SUBINDEX0_NOT_ZERO;
       }
@@ -60,11 +60,11 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
 
    if (ESCvar.pre_object_download_hook)
    {
-      return (ESCvar.pre_object_download_hook) (index,
+      return (ESCvar.pre_object_download_hook)(index,
-            subindex,
+                                               subindex,
-            data,
+                                               data,
-            size,
+                                               size,
-            flags);
+                                               flags);
    }
 
    return 0;
@@ -77,7 +77,7 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
  * @param[in] sub-index  = sub-index of SDO download request to handle
  * @return SDO abort code, or 0 on success
  */
-uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
+uint32_t ESC_download_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
 {
    if (ESCvar.post_object_download_hook != NULL)
    {
@@ -93,19 +93,19 @@ uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint
  * @param[in] sub-index  = sub-index of SDO upload request to handle
  * @return SDO abort code, or 0 on success
  */
-uint32_t ESC_upload_pre_objecthandler (uint16_t index,
+uint32_t ESC_upload_pre_objecthandler(uint16_t index,
-      uint8_t subindex,
+                                      uint8_t subindex,
-      void * data,
+                                      void *data,
-      size_t size,
+                                      size_t size,
-      uint16_t flags)
+                                      uint16_t flags)
 {
    if (ESCvar.pre_object_upload_hook != NULL)
    {
-      return (ESCvar.pre_object_upload_hook) (index,
+      return (ESCvar.pre_object_upload_hook)(index,
-            subindex,
+                                             subindex,
-            data,
+                                             data,
-            size,
+                                             size,
-            flags);
+                                             flags);
    }
 
    return 0;
@@ -118,7 +118,7 @@ uint32_t ESC_upload_pre_objecthandler (uint16_t index,
  * @param[in] sub-index  = sub-index of SDO upload request to handle
  * @return SDO abort code, or 0 on success
  */
-uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
+uint32_t ESC_upload_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
 {
    if (ESCvar.post_object_upload_hook != NULL)
    {
@@ -131,11 +131,11 @@ uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16
 /** Hook called from the slave stack ESC_stopoutputs to act on state changes
  * forcing us to stop outputs. Here we can set them to a safe state.
  */
-void APP_safeoutput (void)
+void APP_safeoutput(void)
 {
-   DPRINT ("APP_safeoutput\n");
+   DPRINT("APP_safeoutput\n");
 
-   if(ESCvar.safeoutput_override != NULL)
+   if (ESCvar.safeoutput_override != NULL)
    {
       (ESCvar.safeoutput_override)();
    }
@@ -143,9 +143,9 @@ void APP_safeoutput (void)
 
 /** Write local process data to Sync Manager 3, Master Inputs.
  */
-void TXPDO_update (void)
+void TXPDO_update(void)
 {
-   if(ESCvar.txpdo_override != NULL)
+   if (ESCvar.txpdo_override != NULL)
    {
       (ESCvar.txpdo_override)();
    }
@@ -153,26 +153,26 @@ void TXPDO_update (void)
    {
       if (MAX_MAPPINGS_SM3 > 0)
       {
-         COE_pdoPack (txpdo, ESCvar.sm3mappings, SMmap3);
+         COE_pdoPack(txpdo, ESCvar.sm3mappings, SMmap3);
       }
-      ESC_write (ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
+      ESC_write(ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
    }
 }
 
 /** Read Sync Manager 2 to local process data, Master Outputs.
  */
-void RXPDO_update (void)
+void RXPDO_update(void)
 {
-   if(ESCvar.rxpdo_override != NULL)
+   if (ESCvar.rxpdo_override != NULL)
    {
       (ESCvar.rxpdo_override)();
    }
    else
    {
-      ESC_read (ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
+      ESC_read(ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
       if (MAX_MAPPINGS_SM2 > 0)
       {
-         COE_pdoUnpack (rxpdo, ESCvar.sm2mappings, SMmap2);
+         COE_pdoUnpack(rxpdo, ESCvar.sm2mappings, SMmap2);
       }
    }
 }
@@ -182,7 +182,7 @@ void RXPDO_update (void)
  *
  * @param[in] watchdogcnt  = new watchdog count value
  */
-void APP_setwatchdog (int watchdogcnt)
+void APP_setwatchdog(int watchdogcnt)
 {
    CC_ATOMIC_SET(ESCvar.watchdogcnt, watchdogcnt);
 }
@@ -191,10 +191,11 @@ void APP_setwatchdog (int watchdogcnt)
  * write ethercat inputs. Implement watch-dog counter to count-out if we have
  * made state change affecting the App.state.
  */
-void DIG_process (uint8_t flags)
+
+void DIG_process(uint16_t ALEvent, uint8_t flags)
 {
    /* Handle watchdog */
-   if((flags & DIG_PROCESS_WD_FLAG) > 0)
+   if ((flags & DIG_PROCESS_WD_FLAG) > 0)
    {
       if (CC_ATOMIC_GET(watchdog) > 0)
       {
@@ -207,7 +208,7 @@ void DIG_process (uint8_t flags)
          DPRINT("DIG_process watchdog expired\n");
          ESC_ALstatusgotoerror((ESCsafeop | ESCerror), ALERR_WATCHDOG);
       }
-      else if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
+      else if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
       {
          CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
       }
@@ -216,15 +217,15 @@ void DIG_process (uint8_t flags)
    /* Handle Outputs */
    if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
    {
-      if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
+      if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
-         (ESCvar.ALevent & ESCREG_ALEVENT_SM2))
+          (ALEvent & ESCREG_ALEVENT_SM2))
       {
          RXPDO_update();
          CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
          /* Set outputs */
          cb_set_outputs();
       }
-      else if (ESCvar.ALevent & ESCREG_ALEVENT_SM2)
+      else if (ALEvent & ESCREG_ALEVENT_SM2)
       {
          RXPDO_update();
       }
@@ -243,7 +244,7 @@ void DIG_process (uint8_t flags)
    /* Handle Inputs */
    if ((flags & DIG_PROCESS_INPUTS_FLAG) > 0)
    {
-      if(CC_ATOMIC_GET(ESCvar.App.state) > 0)
+      if (CC_ATOMIC_GET(ESCvar.App.state) > 0)
       {
          /* Update inputs */
          cb_get_inputs();
@@ -257,7 +258,7 @@ void DIG_process (uint8_t flags)
  * control what interrupts that should be served and re-activated with
  * event mask argument
  */
-void ecat_slv_worker (uint32_t event_mask)
+void ecat_slv_worker(uint32_t event_mask)
 {
    do
    {
@@ -289,21 +290,21 @@ void ecat_slv_worker (uint32_t event_mask)
 
       CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
 
-   }while(ESCvar.ALevent & event_mask);
+   } while (ESCvar.ALevent & event_mask);
 
    ESC_ALeventmaskwrite(ESC_ALeventmaskread() | event_mask);
 }
 
 /*
- * Polling function. It should be called periodically for an application 
+ * Polling function. It should be called periodically for an application
  * when only SM2/DC interrupt is active.
  * Read and handle events for the EtherCAT state, status, mailbox and eeprom.
  */
-void ecat_slv_poll (void)
+void ecat_slv_poll(void)
 {
    /* Read local time from ESC*/
-   ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
+   ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
-   ESCvar.Time = etohl (ESCvar.Time);
+   ESCvar.Time = etohl(ESCvar.Time);
 
    /* Check the state machine */
    ESC_state();
@@ -336,50 +337,50 @@ void ecat_slv_poll (void)
 /*
  * Poll all events in a free-run application
  */
-void ecat_slv (void)
+void ecat_slv(void)
 {
    ecat_slv_poll();
-   DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+   DIG_process(ESC_ALeventread(), DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
-         DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
+                                      DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
 }
 
 /*
  * Initialize the slave stack.
  */
-void ecat_slv_init (esc_cfg_t * config)
+void ecat_slv_init(esc_cfg_t *config)
 {
    /* Init watchdog */
    watchdog = config->watchdog_cnt;
 
    /* Call stack configuration */
-   ESC_config (config);
+   ESC_config(config);
    /* Call HW init */
-   ESC_init (config);
+   ESC_init(config);
 
    /*  wait until ESC is started up */
    while ((ESCvar.DLstatus & 0x0001) == 0)
    {
-      ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
+      ESC_read(ESCREG_DLSTATUS, (void *)&ESCvar.DLstatus,
-                sizeof (ESCvar.DLstatus));
+               sizeof(ESCvar.DLstatus));
-      ESCvar.DLstatus = etohs (ESCvar.DLstatus);
+      ESCvar.DLstatus = etohs(ESCvar.DLstatus);
    }
 
 #if USE_FOE
    /* Init FoE */
-   FOE_init ();
+   FOE_init();
 #endif
 
 #if USE_EOE
    /* Init EoE */
-   EOE_init ();
+   EOE_init();
 #endif
 
    /* reset ESC to init state */
-   ESC_ALstatus (ESCinit);
+   ESC_ALstatus(ESCinit);
-   ESC_ALerror (ALERR_NONE);
+   ESC_ALerror(ALERR_NONE);
-   ESC_stopmbx ();
+   ESC_stopmbx();
-   ESC_stopinput ();
+   ESC_stopinput();
-   ESC_stopoutput ();
+   ESC_stopoutput();
    /* Init Object Dictionary default values */
-   COE_initDefaultValues ();
+   COE_initDefaultValues();
 }

Firmware/lib/soes/ecat_slv.h

@@ -15,7 +15,7 @@
 void cb_get_inputs();
 
 /**
-* This function is called when to set outputs values
+ * This function is called when to set outputs values
  */
 void cb_set_outputs();
 
@@ -23,12 +23,12 @@ void cb_set_outputs();
  *
  * @param[in] watchdogcnt  = new watchdog count value
  */
-void APP_setwatchdog (int watchdogcnt);
+void APP_setwatchdog(int watchdogcnt);
 
-#define DIG_PROCESS_INPUTS_FLAG     0x01
+#define DIG_PROCESS_INPUTS_FLAG 0x01
-#define DIG_PROCESS_OUTPUTS_FLAG    0x02
+#define DIG_PROCESS_OUTPUTS_FLAG 0x02
-#define DIG_PROCESS_WD_FLAG         0x04
+#define DIG_PROCESS_WD_FLAG 0x04
-#define DIG_PROCESS_APP_HOOK_FLAG   0x08
+#define DIG_PROCESS_APP_HOOK_FLAG 0x08
 /** Implements the watch-dog counter to count if we should make a state change
  * due to missing incoming SM2 events. Updates local I/O and run the application
  * in the following order, call read EtherCAT outputs, execute user provided
@@ -36,7 +36,7 @@ void APP_setwatchdog (int watchdogcnt);
  *
  * @param[in]   flags     = User input what to execute
  */
-void DIG_process (uint8_t flags);
+void DIG_process(uint16_t ALEvent, uint8_t flags);
 
 /**
  * Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
@@ -46,24 +46,24 @@ void DIG_process (uint8_t flags);
  * @param[in]   event_mask = Event mask for interrupts to serve and re-activate
  *                           after served
  */
-void ecat_slv_worker (uint32_t event_mask);
+void ecat_slv_worker(uint32_t event_mask);
 
 /**
  * Poll SM0/1, EEPROM and AL CONTROL events in a SM/DC synchronization
  * application
  */
-void ecat_slv_poll (void);
+void ecat_slv_poll(void);
 
 /**
  * Poll all events in a free-run application
  */
-void ecat_slv (void);
+void ecat_slv(void);
 
 /**
  * Initialize the slave stack
  *
  * @param[in]   config     = User input how to configure the stack
  */
-void ecat_slv_init (esc_cfg_t * config);
+void ecat_slv_init(esc_cfg_t *config);
 
 #endif /* __ECAT_SLV_H__ */

Firmware/src/StepGen2.cpp

@@ -31,21 +31,21 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
 {
     frequency = 0;
     nSteps = 0;
-    dbg=0;
+    dbg = 0;
     if (!enabled) // Just .... don't
         return updatePos(0);
 
     commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
+#if 0
     if (initialStepPosition == commandedStepPosition)              // No movement
     {
-
         return updatePos(1);
     }
-
+#endif
     nSteps = commandedStepPosition - initialStepPosition;
     lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
 
-    if (abs(nSteps) < 1)                                                                    // Some small number
+    if (abs(nSteps) < 0)                                                                    // Some small number
     {                                                                                       //
         frequency = (abs(nSteps) + 1) / lcncCycleTime;                                      // Distribute steps inside available time
         Tpulses = abs(nSteps) / frequency;                                                  //
@@ -63,12 +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.
 
     timerFrequency = uint32_t(ceil(frequency));
     startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
+    startTimer->refresh();
     startTimer->resume();
     return 1;
 }
@@ -83,6 +85,7 @@ void StepGen2::startTimerCB()
     pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
     // pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
     pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
+    pulseTimer->refresh();
     pulseTimer->resume();
 }
 
@@ -112,7 +115,7 @@ int64_t extend32to64::extendTime(uint32_t in)
 
     // 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)
+    if (dif < int64_t(-HALF_PERIOD))
         mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
 
     int64_t unwrapped = previousTimeValue + mod_dif;

Firmware/src/main.cpp

@@ -35,9 +35,6 @@ void startTimerCallback2(void) { Step2.startTimerCB(); }
 
 CircularBuffer<uint32_t, 200> Tim;
 volatile uint64_t irqTime = 0, thenTime = 0, nowTime = 0;
-volatile uint64_t EcatTimeIRQ = 0, EcatTimeThen = 0, EcatTimeDiff = 0;
-;
-volatile uint32_t ccnnt = 0;
 extend32to64 longTime;
 
 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
@@ -53,27 +50,28 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
 }
 
 uint16_t nLoops;
+uint64_t reallyNowTime = 0, reallyThenTime = 0;
+uint64_t timeDiff; // Timediff in nanoseconds
 void handleStepper(void)
 {
    if (!(ALEventIRQ & ESCREG_ALEVENT_SM2))
       return;
    // Catch the case when we miss a loop for some reason
-   uint64_t EcatTimeNow;
+   uint32_t t = micros();
-   ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeNow, sizeof(EcatTimeNow));
+   reallyNowTime = longTime.extendTime(t);
-   EcatTimeNow = etohl(EcatTimeNow);
+   timeDiff = 1000 * (reallyNowTime - reallyThenTime);
-   EcatTimeDiff = EcatTimeNow - EcatTimeThen;
+   nLoops = round(timeDiff / double(StepGen2::sync0CycleTime));
-   nLoops = round(EcatTimeDiff / double(StepGen2::sync0CycleTime));
+   reallyThenTime = reallyNowTime;
-   EcatTimeThen = EcatTimeNow;
 
    Step1.enabled = true;
    Step1.commandedPosition = Obj.CommandedPosition1;
    Step1.stepsPerMM = Obj.StepsPerMM1;
-   Step1.handleStepper(irqTime, 1/*nLoops*/);
+   Step1.handleStepper(irqTime, nLoops);
 
    Step2.enabled = true;
    Step2.commandedPosition = Obj.CommandedPosition2;
    Step2.stepsPerMM = Obj.StepsPerMM2;
-   Step2.handleStepper(irqTime, 1/*nLoops*/);
+   Step2.handleStepper(irqTime, nLoops);
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -97,7 +95,7 @@ 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 = abs(Step2.nSteps);
+   Obj.D2 = nLoops;
    Obj.D3 = Step2.Tstartu;
    Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
 }
@@ -145,8 +143,8 @@ void loop(void)
       /* Read local time from ESC*/
       ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
       ESCvar.Time = etohl(ESCvar.Time);
-      DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+      DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
-                  DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
+                                  DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
       serveIRQ = 0;
       ESCvar.PrevTime = ESCvar.Time;
    }
@@ -157,11 +155,8 @@ void loop(void)
 volatile uint32_t cnt = 0;
 void sync0Handler(void)
 {
-   ccnnt++;
    ALEventIRQ = ESC_ALeventread();
    serveIRQ = 1;
-   ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeIRQ, sizeof(EcatTimeIRQ));
-   EcatTimeIRQ = etohl(EcatTimeIRQ);
    irqTime = longTime.extendTime(micros());
 }