work

Co-authored-by: Marcus Carlbom <MarcusCarlbom@users.noreply.github.com>
Co-authored-by: Albin Wallenius Woxnerud <Albin@woxnerud.se>

src/NTPTask.c

@@ -33,6 +33,7 @@ void vStartNTPClientTasks_SingleTasks(uint16_t usTaskStackSize, UBaseType_t uxTa
 static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
 {
     // setup sleep
+    const TickType_t x100ms = 100UL / portTICK_PERIOD_MS;
     const TickType_t x1000ms = 1000UL / portTICK_PERIOD_MS;
     const TickType_t x10000ms = 10000UL / portTICK_PERIOD_MS;
 
@@ -95,7 +96,7 @@ static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
     r->mode = MODE;
     r->stratum = MAXSTRAT;
     r->poll = MINPOLL;
-    r->precision = PRECISION;
+    r->precision = -18;
 
     memset(x, 0, sizeof(ntp_x));
     x->dstaddr = DSTADDR;
@@ -104,13 +105,13 @@ static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
     x->mode = MODE;
     x->stratum = MAXSTRAT;
     x->poll = MINPOLL;
-    x->precision = PRECISION;
+    x->precision = -18;
 
     memset(&s, sizeof(ntp_s), 0);
     s.leap = NOSYNC;
     s.stratum = MAXSTRAT;
     s.poll = MINPOLL;
-    s.precision = PRECISION;
+    s.precision = -18;
     s.p = NULL;
 
     memset(&c, sizeof(ntp_c), 0);
@@ -158,16 +159,14 @@ static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
     // since we dont have local clock we just set the org to the first rec
     // x->org = r->xmt;
     settime(x->xmt);
-    const TickType_t x1700ms = 1700UL / portTICK_PERIOD_MS;
-    vTaskDelay(x1700ms);
     tstamp testTimestamp = gettime();
 
-    printTimestamp(testTimestamp, "test timestamp is:");
+    printTimestamp(testTimestamp, "MARCUS TESTING GETTIME SHOULD BE +700 MS:");
     free(r);
 
     /*
-        get time from all 5 servers once!
-    */
+    get time from all 5 servers once!
+*/
 
     /*
         then just get all the times forever
@@ -193,6 +192,7 @@ static void vNTPTaskSendUsingStandardInterface(void *pvParameters)
             uint32_t orgfrac = (uint32_t)(x->xmt & 0xFFFFFFFF);
             FreeRTOS_printf(("\n\n sent x xmt to : %s.%u\n", ctime(&orgtimeInSeconds), orgfrac));
 
+            x->xmt = gettime();
             xmit_packet(x);
             r = malloc(sizeof(ntp_r));
             r = recv_packet();

src/NTP_TDMG.c

@@ -130,13 +130,22 @@ double sqrt(double number)
     return (low + high) / 2;
 }
 
-uint64_t subtract_uint64_t(uint64_t x, uint64_t y)
+int64_t subtract_uint64_t(uint64_t x, uint64_t y)
 {
+    int neg = 0;
     uint32_t xFrac = (uint32_t)(x & 0xFFFFFFFF);
     uint32_t yFrac = (uint32_t)(y & 0xFFFFFFFF);
     uint32_t xS = (uint32_t)(x >> 32);
     uint32_t yS = (uint32_t)(y >> 32);
 
+    if (xS < yS || (xS == yS && xFrac < yFrac))
+    {
+        neg = 1;
+        uint64_t temp = x;
+        x = y;
+        y = temp;
+    }
+
     int borrow = 0;
     if (xFrac < yFrac)
     {
@@ -153,26 +162,32 @@ uint64_t subtract_uint64_t(uint64_t x, uint64_t y)
     uint32_t high = xS - yS - borrow;
 
     uint64_t result = ((uint64_t)high << 32) | low;  // Assemble the high and low parts back into a 64-bit integer
+    result = (neg) ? -result : result;               // Apply the sign if necessary
 
     return result;
 }
 
-uint64_t add_uint64_t(uint64_t x, uint64_t y)
+int64_t add_int64_t(int64_t x, int64_t y)
 {
-    uint32_t xHigh = (uint32_t)(x >> 32);        // High 32 bits of x
-    uint32_t xLow = (uint32_t)(x & 0xFFFFFFFF);  // Low 32 bits of x
-    uint32_t yHigh = (uint32_t)(y >> 32);        // High 32 bits of y
-    uint32_t yLow = (uint32_t)(y & 0xFFFFFFFF);  // Low 32 bits of y
+    int32_t xFrac = (int32_t)(x & 0xFFFFFFFF);
+    int32_t yFrac = (int32_t)(y & 0xFFFFFFFF);
+    int32_t xS = (int32_t)(x >> 32);
+    int32_t yS = (int32_t)(y >> 32);
+
+    if (xFrac + yFrac < xFrac || xFrac + yFrac < yFrac)
+    {
+        xS++;
+    }
 
-    // Add the low parts and check for overflow
-    uint32_t sumLow = xLow + yLow;
-    uint32_t carry = sumLow < xLow ? 1 : 0;  // If overflow occurred, set carry
+    int32_t low = xFrac + yFrac;
+    // FreeRTOS_printf_wrapper("", xFrac);
+    // FreeRTOS_printf_wrapper("", yFrac);
+    // FreeRTOS_printf_wrapper_("", low);
 
-    // Add the high parts and include the carry
-    uint32_t sumHigh = xHigh + yHigh + carry;
+    int32_t high = xS + yS;
+    FreeRTOS_printf_wrapper_double("", high);
 
-    // Combine the high and low parts into a 64-bit result
-    uint64_t result = ((uint64_t)sumHigh << 32) | sumLow;
+    int64_t result = ((int64_t)high << 32) | low;  // Assemble the high and low parts back into a 64-bit integer
 
     return result;
 }
@@ -286,7 +301,7 @@ tstamp gettime()
     TickType_t numMilliseconds = tickDifference % 1000;
 
     // Convert milliseconds to fraction part of NTP timestamp
-    uint32_t newFractions = (numMilliseconds * 0xFFFFFFFF) / 1000;
+    uint32_t newFractions = numMilliseconds * (FRAC / 1000);
 
     // Extract current fractions and seconds from last timestamp
     uint32_t currentFractions = (uint32_t)(lastTimeStampTstamp & 0xFFFFFFFF);
@@ -328,14 +343,15 @@ void printTimestamp(tstamp timestamp, const char *comment)
     uint32_t fractions = (timestamp & 0xFFFFFFFF);
 
     // double fractionAsSecond = fractions / (double)0xFFFFFFFF;
-    double fractionAsSecond = fractions / 4294967296.0;  // / 4294967296.0 = 2^32
+    double fractionAsSecond = (double)fractions / FRAC;  // / 4294967296.0 = 2^32
 
     // time_t timeInSeconds = (time_t)((r->rec >> 32) - 2208988800ull);
     // uint32_t frac = (uint32_t)(r->rec & 0xFFFFFFFF);
     // FreeRTOS_printf(("\n\n Time according to %s: %s.%u\n", pcHostNames[i], ctime(&timeInSeconds), frac));
 
     // Print the timestamp with the comment
-    FreeRTOS_printf(("%s Timestamp: %s.%.10d seconds\n", comment, ctime(&seconds), fractionAsSecond));
+    FreeRTOS_printf(("%s Timestamp: %s. seconds\n", comment, ctime(&seconds)));
+    FreeRTOS_printf_wrapper_double("", fractionAsSecond);
 }
 
 #define MAX_UINT64_DIGITS 20  // Maximum digits in uint64_t
@@ -444,7 +460,7 @@ void double_to_str(double value, char *str, int precision)
 void FreeRTOS_printf_wrapper_double(const char *format, double value)
 {
     char buffer[MAX_DOUBLE_DIGITS + 50];  // Additional space for message text
-    double_to_str(value, buffer, 6);      // Example with 6 decimal places
+    double_to_str(value, buffer, 10);     // Example with 6 decimal places
     FreeRTOS_printf(("%s\n", buffer));
 }
 

src/NTP_TDMG.h

@@ -65,19 +65,19 @@ typedef int8_t s_char;   /* precision and poll interval (log2) */
 /*
  * Timestamp conversion macroni
  */
-#define FRIC 65536.                 /* 2^16 as a double */
-#define D2FP(r) ((tdist)((r)*FRIC)) /* NTP short */
+#define FRIC 65536.                   /* 2^16 as a double */
+#define D2FP(r) ((tdist)((r) * FRIC)) /* NTP short */
 #define FP2D(r) ((double)(r) / FRIC)
 
-#define FRAC 4294967296.              /* 2^32 as a double */
-#define D2LFP(a) ((tstamp)((a)*FRAC)) /* NTP timestamp */
+#define FRAC 4294967296.                /* 2^32 as a double */
+#define D2LFP(a) ((tstamp)((a) * FRAC)) /* NTP timestamp */
 #define LFP2D(a) ((double)(a) / FRAC)
 #define U2LFP(a) (((unsigned long long)((a).tv_sec + JAN_1970) << 32) + (unsigned long long)((a).tv_usec / 1e6 * FRAC))
 
 /*
  * Arithmetic conversions
  */
-#define LOG2D(a) ((a) < 0 ? 1. / (1L << -(a)) : 1L << (a)) /* poll, etc. */
+#define LOG2D(a) ((a) < 0 ? (double)(1. / (1L << -(a))) : (double)(1L << (a))) /* poll, etc. */
 #define SQUARE(x) (x * x)
 #define SQRT(x) (sqrt(x))
 // TODO THIS IS A DUMMY IMPLEMENTATION JUST TO MAKE THE CODE COMPILE
@@ -355,7 +355,7 @@ typedef struct ntp_s
     char leap;            /* leap indicator */
     char stratum;         /* stratum */
     char poll;            /* poll interval */
-    char precision;       /* precision */
+    s_char precision;     /* precision */
     double rootdelay;     /* root delay */
     double rootdisp;      /* root dispersion */
     char refid;           /* reference ID */
@@ -431,8 +431,8 @@ void ntp_init();
 
 double sqrt(double number);
 
-uint64_t subtract_uint64_t(uint64_t x, uint64_t y);
-uint64_t add_uint64_t(uint64_t x, uint64_t y);
+int64_t subtract_uint64_t(uint64_t x, uint64_t y);
+int64_t add_int64_t(int64_t x, int64_t y);
 
 void settime(tstamp newTime);
 tstamp gettime();

src/NTP_peer.c

@@ -7,12 +7,17 @@
 
 // A.5.1.1.  packet()
 
-/*
-DUMMY SOLUTION
-table should be in the header file but the current configuration and dependency tree make it hard
-TODO
-*/
-
+double log2d(int a)
+{
+    if (a < 0)
+    {
+        return 1.0 / (1L << -a);
+    }
+    else
+    {
+        return (double)(1L << a);
+    }
+}
 /*
  * Dispatch matrix
  *              active  passv  client server bcast */
@@ -145,9 +150,8 @@ void packet(struct ntp_p *p, /* peer structure pointer */
         //         disp = LOG2D(r->precision) + LOG2D(s.precision) + PHI * LFP2D(r->dst - r->org);
         //
 
-        offset = (double)add_uint64_t(LFP2D((double)subtract_uint64_t(r->rec, r->org)),
-                                      LFP2D((double)subtract_uint64_t(r->dst, r->xmt))) /
-                 2;
+        offset = LFP2D(add_int64_t(subtract_uint64_t(r->rec, r->org), subtract_uint64_t(r->dst, r->xmt))) /
+                 2;  // TODO SHOULD BE DIVIED BY 2
 
         delay = max((double)(subtract_uint64_t(LFP2D((double)subtract_uint64_t(r->dst, r->org)),
                                                LFP2D((double)subtract_uint64_t(r->rec, r->xmt)))),
@@ -158,10 +162,15 @@ void packet(struct ntp_p *p, /* peer structure pointer */
     // double tempOffset3 = 2;
     // double printedmessage = tempOffset2/tempOffset3;
     // FreeRTOS_printf_wrapper_double("\n\n\n lets see if offset is working naaow: %s", printedmessage);
-
-    FreeRTOS_printf_wrapper_double("\n\n\n lets see if offset is working naaow: %s", offset);
-    FreeRTOS_printf_wrapper_double("\n\n\n lets see if offset is working naaow: %s", delay);
-    FreeRTOS_printf_wrapper_double("\n\n\n lets see if offset is working naaow: %s", disp);
+    print_uint64_as_32_parts(r->rec);
+    print_uint64_as_32_parts(r->org);
+    print_uint64_as_32_parts(r->dst);
+    print_uint64_as_32_parts(r->xmt);
+    FreeRTOS_printf_wrapper_double("",
+                                   add_int64_t(subtract_uint64_t(r->rec, r->org), subtract_uint64_t(r->dst, r->xmt)));
+    FreeRTOS_printf_wrapper_double("", offset);
+    FreeRTOS_printf_wrapper_double("", delay);
+    FreeRTOS_printf_wrapper_double("", disp);
     // FreeRTOS_printf(("\n\n\n lets see if offset is working: %d\n\n\n", offset)); // = 0
     // FreeRTOS_printf(("\n\n\ndelay is %d\n\n\n", delay));
     // FreeRTOS_printf(("\n\n\ndisp is %d\n\n\n", disp));