diff --git a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
index c5c85b5..33ac9ea 100644
--- a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
+++ b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
@@ -32,6 +32,7 @@
 #include <time.h>
 #include <limits.h>
 #include <stdio.h>
+#include <string.h>
 
 /*****************************************************************************/
 /*    here: Defines                                                          */
@@ -47,16 +48,33 @@
 // function: diffUSec
 // ****************************************************************************
 
-static inline int diffUSec(struct timespec t1, struct timespec t2)
+static inline int diffUSec(struct timespec t2, struct timespec t1)
 {
+#define NS_IN_ONE_SEC          (1000000000LL)
+#define SEC_TO_NANOSEC(s) ((s)*NS_IN_ONE_SEC)
+#if 1
+       struct timespec diff;
+       if ((t2.tv_nsec-t1.tv_nsec) < 0) {
+               diff.tv_sec  = t2.tv_sec - t1.tv_sec - 1;
+               diff.tv_nsec = t2.tv_nsec - t1.tv_nsec + 1000000000;
+       } else {
+               diff.tv_sec  = t2.tv_sec - t1.tv_sec;
+               diff.tv_nsec = t2.tv_nsec - t1.tv_nsec;
+       }
+       return (diff.tv_sec * 1000000.0 + (diff.tv_nsec / 1000.0));
+#else
    int diff;
 
    // calculate and return the difference of t1 and t2 in microseconds
 
    diff = USEC_PER_SEC * ((int) t1.tv_sec - (int) t2.tv_sec);
-   diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
+   if (t1.tv_nsec > t2.tv_nsec)
+       diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
+   else
+       diff += (((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000)+USEC_PER_SEC;
 
    return diff;
+#endif
 }
 
 // ****************************************************************************
@@ -245,6 +263,7 @@ Ecu01SoftCompCap::Ecu01SoftCompCap():
 {
    int prio = sched_get_priority_max(SCHED_FIFO);
    struct sched_param param;
+   (void)memset(&param, 0, sizeof(param));
 
    param.sched_priority = prio;