jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed bug in extend32to64:extendTime()

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-13 23:33:25 +01:00
parent f341eb5074
commit 71ae242fc4
5 changed files with 109 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Firmware/include/StepGen2.h
View File

@@ -53,7 +53,7 @@ class extend32to64
public:
int64_t previousTimeValue = 0;
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);
};

9
Firmware/lib/soes/ecat_slv.c
View File

@@ -191,7 +191,8 @@ 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)
@@ -217,14 +218,14 @@ void DIG_process (uint8_t flags)
if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 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();
}
@@ -339,7 +340,7 @@ void ecat_slv_poll (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);
}

2
Firmware/lib/soes/ecat_slv.h
View File

@@ -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

11
Firmware/src/StepGen2.cpp
View File

@@ -36,16 +36,16 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
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;

27
Firmware/src/main.cpp
View File

@@ -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;
ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeNow, sizeof(EcatTimeNow));
EcatTimeNow = etohl(EcatTimeNow);
EcatTimeDiff = EcatTimeNow - EcatTimeThen;
nLoops = round(EcatTimeDiff / double(StepGen2::sync0CycleTime));
EcatTimeThen = EcatTimeNow;
uint32_t t = micros();
reallyNowTime = longTime.extendTime(t);
timeDiff = 1000 * (reallyNowTime - reallyThenTime);
nLoops = round(timeDiff / double(StepGen2::sync0CycleTime));
reallyThenTime = reallyNowTime;
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,7 +143,7 @@ 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);
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());
}