Diff: VIPSSerialProtocol.cpp

Revision:

13:c2d9e975841b

Parent:

10:053bac3e326b

Child:

14:76083dc18b0d

diff -r 06050debf014 -r c2d9e975841b VIPSSerialProtocol.cpp
--- a/VIPSSerialProtocol.cpp	Fri Feb 19 16:53:27 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Mon Feb 22 10:44:27 2021 +0000
@@ -329,6 +329,10 @@
         return NULL;
     lastPoints = pointCount;
 
+    struct posAndTime_s lastPos; // the most recent position received
+    struct posAndTime_s prevPos; // the most last but one position received
+
+    __disable_irq();   // disable IRQ and make a copy of the data we're going to interpolate.
     int lastPoint = nextPosition - 1;
     int prevPoint = nextPosition - 2;
     if (lastPoint<0)
@@ -336,34 +340,30 @@
     if (prevPoint<0)
         prevPoint+=posHistoryLen;
 
+    memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s));
+    memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s));
+    __enable_irq();
+
     // calculate timestamps as a function of time since last frame
 
-    uint64_t LastTimeMark = lastPositions[lastPoint].time;
-    uint64_t PrevTimeMark = lastPositions[prevPoint].time;
+    uint64_t LastTimeMark = lastPos.time;
+    uint64_t PrevTimeMark = prevPos.time;
     uint64_t timeValueUnwrap = timeValue;
     if (PrevTimeMark > LastTimeMark)
         LastTimeMark += ((uint64_t)1)<<32;
     if (LastTimeMark > timeValueUnwrap)
         timeValueUnwrap += ((uint64_t)1)<<32;
 
-    outputPosition.time = timeValueUnwrap-PrevTimeMark;
+    outputPosition.time = timeValueUnwrap-PrevTimeMark; // should be between 10,000 and 20,000
 
-    int32_t lTime = lastPositions[lastPoint].pos.time;
-    int32_t pTime = lastPositions[prevPoint].pos.time;
-
-    lastPositions[lastPoint].pos.time = LastTimeMark-PrevTimeMark;
-    lastPositions[prevPoint].pos.time = 0;
+    // interpolate uses the position times. Replace them with the internal clock counts.
+    lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000
+    prevPos.pos.time = 0;
 
     // interpolate position to requested time.
-    if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) {
-        lastPositions[lastPoint].pos.time = lTime;
-        lastPositions[prevPoint].pos.time = pTime;
+    if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
         return &outputPosition;
     }
-
-    lastPositions[lastPoint].pos.time = lTime;
-    lastPositions[prevPoint].pos.time = pTime;
-
     return NULL;
 }