From 34e60e43d8a1c3aa68c10f6f7ad4220fb58d6360 Mon Sep 17 00:00:00 2001
From: Hakan Bastedt <hakan.bastedt@gmail.com>
Date: Wed, 3 Apr 2024 21:21:51 +0200
Subject: [PATCH] Adaptive max cycle time adjustment, use maxCycleTime

---
 Firmware/src/main.cpp | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index f4a108e..3006a2d 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -118,10 +118,10 @@ void handleStepper(void)
    Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
    Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
    uint32_t diffT = longTime.extendTime(micros()) - irqTime;
-   delayT = 500 - diffT;
+   delayT = maxCycleTime + 50 - diffT - jitterThisCycle; // Add 50 as some saftey margin
    if (delayT > 0 && delayT < 900)
    {
-      syncTimer->setOverflow(delayT, MICROSEC_FORMAT);
+      syncTimer->setOverflow(delayT, MICROSEC_FORMAT); // Work in flawed units, its ok
       syncTimer->refresh();
       syncTimer->resume();
    }
@@ -247,12 +247,8 @@ void sync0Handler(void)
 {
    irqTime = longTime.extendTime(micros());
    ALEventIRQ = ESC_ALeventread();
-   // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
-   {
-      serveIRQ = 1;
-
-      irqCnt++;
-   }
+   serveIRQ = 1;
+   irqCnt++;
 }
 
 // Enable SM2 interrupts