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);
};

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

@@ -13,7 +13,7 @@
#define IS_TXPDO(index) ((index) >= 0x1A00 && (index) < 0x1C00)
/* Global variables used by the stack */
uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE,MBXSIZEBOOT)];
uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE, MBXSIZEBOOT)];
_MBXcontrol MBXcontrol[MBXBUFFERS];
_SMmap SMmap2[MAX_MAPPINGS_SM2];
_SMmap SMmap3[MAX_MAPPINGS_SM3];
@@ -23,15 +23,15 @@ _ESCvar ESCvar;
static volatile int watchdog;
#if MAX_MAPPINGS_SM2 > 0
static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned (8)));
static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned(8)));
#else
extern uint8_t * rxpdo;
extern uint8_t *rxpdo;
#endif
#if MAX_MAPPINGS_SM3 > 0
static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned (8)));
static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned(8)));
#else
extern uint8_t * txpdo;
extern uint8_t *txpdo;
#endif
/** Function to pre-qualify the incoming SDO download.
@@ -40,19 +40,19 @@ extern uint8_t * txpdo;
* @param[in] sub-index = sub-index of SDO download request to check
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_download_pre_objecthandler (uint16_t index,
uint32_t ESC_download_pre_objecthandler(uint16_t index,
uint8_t subindex,
void * data,
void *data,
size_t size,
uint16_t flags)
{
if (IS_RXPDO (index) ||
IS_TXPDO (index) ||
if (IS_RXPDO(index) ||
IS_TXPDO(index) ||
index == RX_PDO_OBJIDX ||
index == TX_PDO_OBJIDX)
{
uint8_t minSub = ((flags & COMPLETE_ACCESS_FLAG) == 0) ? 0 : 1;
if (subindex > minSub && COE_maxSub (index) != 0)
if (subindex > minSub && COE_maxSub(index) != 0)
{
return ABORT_SUBINDEX0_NOT_ZERO;
}
@@ -60,7 +60,7 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
if (ESCvar.pre_object_download_hook)
{
return (ESCvar.pre_object_download_hook) (index,
return (ESCvar.pre_object_download_hook)(index,
subindex,
data,
size,
@@ -77,7 +77,7 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
* @param[in] sub-index = sub-index of SDO download request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
uint32_t ESC_download_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
{
if (ESCvar.post_object_download_hook != NULL)
{
@@ -93,15 +93,15 @@ uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint
* @param[in] sub-index = sub-index of SDO upload request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_upload_pre_objecthandler (uint16_t index,
uint32_t ESC_upload_pre_objecthandler(uint16_t index,
uint8_t subindex,
void * data,
void *data,
size_t size,
uint16_t flags)
{
if (ESCvar.pre_object_upload_hook != NULL)
{
return (ESCvar.pre_object_upload_hook) (index,
return (ESCvar.pre_object_upload_hook)(index,
subindex,
data,
size,
@@ -118,7 +118,7 @@ uint32_t ESC_upload_pre_objecthandler (uint16_t index,
* @param[in] sub-index = sub-index of SDO upload request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
uint32_t ESC_upload_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
{
if (ESCvar.post_object_upload_hook != NULL)
{
@@ -131,11 +131,11 @@ uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16
/** Hook called from the slave stack ESC_stopoutputs to act on state changes
* forcing us to stop outputs. Here we can set them to a safe state.
*/
void APP_safeoutput (void)
void APP_safeoutput(void)
{
DPRINT ("APP_safeoutput\n");
DPRINT("APP_safeoutput\n");
if(ESCvar.safeoutput_override != NULL)
if (ESCvar.safeoutput_override != NULL)
{
(ESCvar.safeoutput_override)();
}
@@ -143,9 +143,9 @@ void APP_safeoutput (void)
/** Write local process data to Sync Manager 3, Master Inputs.
*/
void TXPDO_update (void)
void TXPDO_update(void)
{
if(ESCvar.txpdo_override != NULL)
if (ESCvar.txpdo_override != NULL)
{
(ESCvar.txpdo_override)();
}
@@ -153,26 +153,26 @@ void TXPDO_update (void)
{
if (MAX_MAPPINGS_SM3 > 0)
{
COE_pdoPack (txpdo, ESCvar.sm3mappings, SMmap3);
COE_pdoPack(txpdo, ESCvar.sm3mappings, SMmap3);
}
ESC_write (ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
ESC_write(ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
}
}
/** Read Sync Manager 2 to local process data, Master Outputs.
*/
void RXPDO_update (void)
void RXPDO_update(void)
{
if(ESCvar.rxpdo_override != NULL)
if (ESCvar.rxpdo_override != NULL)
{
(ESCvar.rxpdo_override)();
}
else
{
ESC_read (ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
ESC_read(ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
if (MAX_MAPPINGS_SM2 > 0)
{
COE_pdoUnpack (rxpdo, ESCvar.sm2mappings, SMmap2);
COE_pdoUnpack(rxpdo, ESCvar.sm2mappings, SMmap2);
}
}
}
@@ -182,7 +182,7 @@ void RXPDO_update (void)
*
* @param[in] watchdogcnt = new watchdog count value
*/
void APP_setwatchdog (int watchdogcnt)
void APP_setwatchdog(int watchdogcnt)
{
CC_ATOMIC_SET(ESCvar.watchdogcnt, watchdogcnt);
}
@@ -191,10 +191,11 @@ 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)
if ((flags & DIG_PROCESS_WD_FLAG) > 0)
{
if (CC_ATOMIC_GET(watchdog) > 0)
{
@@ -207,7 +208,7 @@ void DIG_process (uint8_t flags)
DPRINT("DIG_process watchdog expired\n");
ESC_ALstatusgotoerror((ESCsafeop | ESCerror), ALERR_WATCHDOG);
}
else if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
else if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
{
CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
}
@@ -216,15 +217,15 @@ void DIG_process (uint8_t flags)
/* Handle Outputs */
if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
{
if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
(ESCvar.ALevent & ESCREG_ALEVENT_SM2))
if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
(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();
}
@@ -243,7 +244,7 @@ void DIG_process (uint8_t flags)
/* Handle Inputs */
if ((flags & DIG_PROCESS_INPUTS_FLAG) > 0)
{
if(CC_ATOMIC_GET(ESCvar.App.state) > 0)
if (CC_ATOMIC_GET(ESCvar.App.state) > 0)
{
/* Update inputs */
cb_get_inputs();
@@ -257,7 +258,7 @@ void DIG_process (uint8_t flags)
* control what interrupts that should be served and re-activated with
* event mask argument
*/
void ecat_slv_worker (uint32_t event_mask)
void ecat_slv_worker(uint32_t event_mask)
{
do
{
@@ -289,7 +290,7 @@ void ecat_slv_worker (uint32_t event_mask)
CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
}while(ESCvar.ALevent & event_mask);
} while (ESCvar.ALevent & event_mask);
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | event_mask);
}
@@ -299,11 +300,11 @@ void ecat_slv_worker (uint32_t event_mask)
* when only SM2/DC interrupt is active.
* Read and handle events for the EtherCAT state, status, mailbox and eeprom.
*/
void ecat_slv_poll (void)
void ecat_slv_poll(void)
{
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
ESCvar.Time = etohl(ESCvar.Time);
/* Check the state machine */
ESC_state();
@@ -336,50 +337,50 @@ void ecat_slv_poll (void)
/*
* Poll all events in a free-run application
*/
void ecat_slv (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);
}
/*
* Initialize the slave stack.
*/
void ecat_slv_init (esc_cfg_t * config)
void ecat_slv_init(esc_cfg_t *config)
{
/* Init watchdog */
watchdog = config->watchdog_cnt;
/* Call stack configuration */
ESC_config (config);
ESC_config(config);
/* Call HW init */
ESC_init (config);
ESC_init(config);
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
ESC_read(ESCREG_DLSTATUS, (void *)&ESCvar.DLstatus,
sizeof(ESCvar.DLstatus));
ESCvar.DLstatus = etohs(ESCvar.DLstatus);
}
#if USE_FOE
/* Init FoE */
FOE_init ();
FOE_init();
#endif
#if USE_EOE
/* Init EoE */
EOE_init ();
EOE_init();
#endif
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
ESC_ALstatus(ESCinit);
ESC_ALerror(ALERR_NONE);
ESC_stopmbx();
ESC_stopinput();
ESC_stopoutput();
/* Init Object Dictionary default values */
COE_initDefaultValues ();
COE_initDefaultValues();
}

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

@@ -15,7 +15,7 @@
void cb_get_inputs();
/**
* This function is called when to set outputs values
* This function is called when to set outputs values
*/
void cb_set_outputs();
@@ -23,7 +23,7 @@ void cb_set_outputs();
*
* @param[in] watchdogcnt = new watchdog count value
*/
void APP_setwatchdog (int watchdogcnt);
void APP_setwatchdog(int watchdogcnt);
#define DIG_PROCESS_INPUTS_FLAG 0x01
#define DIG_PROCESS_OUTPUTS_FLAG 0x02
@@ -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
@@ -46,24 +46,24 @@ void DIG_process (uint8_t flags);
* @param[in] event_mask = Event mask for interrupts to serve and re-activate
* after served
*/
void ecat_slv_worker (uint32_t event_mask);
void ecat_slv_worker(uint32_t event_mask);
/**
* Poll SM0/1, EEPROM and AL CONTROL events in a SM/DC synchronization
* application
*/
void ecat_slv_poll (void);
void ecat_slv_poll(void);
/**
* Poll all events in a free-run application
*/
void ecat_slv (void);
void ecat_slv(void);
/**
* Initialize the slave stack
*
* @param[in] config = User input how to configure the stack
*/
void ecat_slv_init (esc_cfg_t * config);
void ecat_slv_init(esc_cfg_t *config);
#endif /* __ECAT_SLV_H__ */

13
Firmware/src/StepGen2.cpp
View File

@@ -31,21 +31,21 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
{
frequency = 0;
nSteps = 0;
dbg=0;
dbg = 0;
if (!enabled) // Just .... don't
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());
}