ln-dotnet
/
SOES
mirror of https://github.com/OpenEtherCATsociety/SOES.git
3
0
Fork 0
Code Issues Projects Releases Wiki Activity

Add IRQ with SM/DC sync0 support

pull/32/head
rtlaka 2017-10-25 14:38:38 +02:00
parent 7715a2fffd
commit 148cdba265
4 changed files with 516 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

383
soes/esc.c
View File

@ -52,6 +52,51 @@ void ESC_ALstatus (uint8_t status)
ESC_write (ESCREG_ALSTATUS, &dummy, sizeof (dummy));
}
/** Write ALeventMask register 0x204.
*
* @param[in] n = AL Event Mask
*/
void ESC_ALeventmaskwrite (uint32_t mask)
{
uint32_t aleventmask;
aleventmask = htoel(mask);
ESC_write (ESCREG_ALEVENTMASK, &aleventmask, sizeof(aleventmask));
}
/** Read AleventMask register 0x204.
*
* @return value of register AL Event Mask
*/
uint32_t ESC_ALeventmaskread (void)
{
uint32_t aleventmask;
ESC_read (ESCREG_ALEVENTMASK, &aleventmask, sizeof(aleventmask));
return htoel(aleventmask);
}
/** Write ALevent register 0x220.
*
* @param[in] n = AL Event Mask
*/
void ESC_ALeventwrite (uint32_t event)
{
uint32_t alevent;
alevent = htoel(event);
ESC_write (ESCREG_ALEVENT, &alevent, sizeof(alevent));
}
/** Read Alevent register 0x220.
*
* @return value of register AL Event Mask
*/
uint32_t ESC_ALeventread (void)
{
uint32_t alevent;
ESC_read (ESCREG_ALEVENT, &alevent, sizeof(alevent));
return htoel(alevent);
}
/** Read SM Status register 0x805(+ offset to SyncManager n) to acknowledge a
* Sync Manager event Bit 3 in ALevent. The result is not used.
*
@ -139,6 +184,107 @@ uint8_t ESC_WDstatus (void)
return (uint8_t) wdstatus;
}
/** Read SYNC Out Unit activation registers 0x981
*
* @return value of register Activation.
*/
uint8_t ESC_SYNCactivation (void)
{
uint8_t activation;
ESC_read (ESCREG_SYNC_ACT, &activation, sizeof(activation));
return activation;
}
/** Read SYNC0 cycle time
*
* @return value of register SYNC0 cycle time
*/
uint32_t ESC_SYNC0cycletime (void)
{
uint32_t cycletime;
ESC_read (ESCREG_SYNC0_CYCLE_TIME, &cycletime, sizeof(cycletime));
cycletime = etohl (cycletime);
return cycletime;
}
/** Read SYNC1 cycle time
*
* @return value of register SYNC1 cycle time
*/
uint32_t ESC_SYNC1cycletime (void)
{
uint32_t cycletime;
ESC_read (ESCREG_SYNC1_CYCLE_TIME, &cycletime, 4);
cycletime = etohl (cycletime);
return cycletime;
}
/** Validate the DC values if the SYNC unit is activated.
*
* @return = 0 if OK, else ERROR code to be set by caller.
*/
uint16_t ESC_checkDC (void)
{
uint16_t ret = 0;
uint8_t sync_act = ESC_SYNCactivation();
uint32_t sync0_cycletime = ESC_SYNC0cycletime();
uint16_t sync_type_supported1c32 = 0;
uint32_t mincycletime = 0;
/* Do we need to check sync settings? */
if((sync_act & (ESCREG_SYNC_ACT_ACTIVATED | ESCREG_SYNC_AUTO_ACTIVATED)) > 0)
{
/* If the sync unit is active at least on signal should be activated */
if(COE_getSyncMgrPara(0x1c32, 0x4, &sync_type_supported1c32, DTYPE_UNSIGNED16) == 0)
{
ret = ALERR_DCINVALIDSYNCCFG;
}
else if(COE_getSyncMgrPara(0x1c32, 0x5, &mincycletime, DTYPE_UNSIGNED32) == 0)
{
ret = ALERR_DCINVALIDSYNCCFG;
}
else if(COE_getSyncMgrPara(0x10F1, 0x2, &sync_counter_limit, DTYPE_UNSIGNED16) == 0)
{
ret = ALERR_DCINVALIDSYNCCFG;
}
else if((sync_act & (ESCREG_SYNC_SYNC0_EN | ESCREG_SYNC_SYNC1_EN)) == 0)
{
ret = ALERR_DCINVALIDSYNCCFG;
}
/* Do we support activated signals */
else if(((sync_type_supported1c32 & SYNCTYPE_SUPPORT_DCSYNC0) == 0) &&
((sync_act & ESCREG_SYNC_SYNC0_EN) > 0))
{
ret = ALERR_DCINVALIDSYNCCFG;
}
/* Do we support activated signals */
else if(((sync_type_supported1c32 & SYNCTYPE_SUPPORT_DCSYNC1) == 0) &&
((sync_act & ESCREG_SYNC_SYNC1_EN) > 0))
{
ret = ALERR_DCINVALIDSYNCCFG;
}
else if((sync0_cycletime != 0) && (sync0_cycletime < mincycletime))
{
ret = ALERR_DCSYNC0CYCLETIME;
}
else
{
dc_sync = 1;
sync_counter = 0;
}
}
else
{
dc_sync = 0;
sync_counter = 0;
}
return ret;
}
/** Check mailbox status by reading all SyncManager 0 and 1 data. The read values
* are compared with local definitions for SM Physical Address, SM Length and SM Control.
* If we check fails we disable Mailboxes by disabling SyncManager 0 and 1 and return
@ -459,7 +605,7 @@ uint8_t ESC_mbxprocess (void)
if (ESCvar.mbxoutpost || ESCvar.mbxbackup)
{
/* if outmbx empty */
if (!ESCvar.mbxoutpost)
if (ESCvar.mbxoutpost == 0)
{
/* use backup mbx */
ESC_writembx (ESCvar.mbxbackup);
@ -502,8 +648,8 @@ uint8_t ESC_mbxprocess (void)
}
/* read mailbox if full and no xoe in progress */
if (ESCvar.SM[0].MBXstat && !MBXcontrol[0].state && !ESCvar.mbxoutpost
&& !ESCvar.xoe)
if ((ESCvar.SM[0].MBXstat != 0) && (MBXcontrol[0].state == 0)
&& (ESCvar.mbxoutpost == 0) && (ESCvar.xoe == 0))
{
ESC_readmbx ();
ESCvar.SM[0].MBXstat = 0;
@ -522,15 +668,6 @@ uint8_t ESC_mbxprocess (void)
return 1;
}
/* ack changes in non used SM */
if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
ESC_SMack (4);
ESC_SMack (5);
ESC_SMack (6);
ESC_SMack (7);
}
return 0;
}
/** Handler for incorrect or unsupported mailbox data. Write error response
@ -598,11 +735,13 @@ uint8_t ESC_checkSM23 (uint8_t state)
*/
uint8_t ESC_startinput (uint8_t state)
{
state = ESC_checkSM23 (state);
if (state != (ESCpreop | ESCerror))
{
ESC_SMenable (3);
App.state = APPSTATE_INPUT;
CC_ATOMIC_SET(App.state, APPSTATE_INPUT);
}
else
{
@ -617,6 +756,42 @@ uint8_t ESC_startinput (uint8_t state)
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
}
/* Exit here if polling */
if (esc_cfg->use_interrupt == 0)
{
return state;
}
if (state != (ESCpreop | ESCerror))
{
uint16_t dc_check_result;
dc_check_result = ESC_checkDC();
if(dc_check_result > 0)
{
ESC_ALerror (dc_check_result);
state = (ESCpreop | ESCerror);
ESC_SMdisable (2);
ESC_SMdisable (3);
CC_ATOMIC_SET(App.state, APPSTATE_IDLE);
}
else
{
if (esc_cfg->esc_hw_interrupt_enable != NULL)
{
if(dc_sync > 0)
{
esc_cfg->esc_hw_interrupt_enable(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2);
}
else
{
esc_cfg->esc_hw_interrupt_enable(ESCREG_ALEVENT_SM2);
}
}
}
}
return state;
}
@ -626,9 +801,16 @@ uint8_t ESC_startinput (uint8_t state)
*/
void ESC_stopinput (void)
{
App.state = APPSTATE_IDLE;
CC_ATOMIC_SET(App.state, APPSTATE_IDLE);
ESC_SMdisable (3);
ESC_SMdisable (2);
/* Call interrupt disable hook case it have been configured */
if ((esc_cfg->use_interrupt != 0) &&
(esc_cfg->esc_hw_interrupt_disable != NULL))
{
esc_cfg->esc_hw_interrupt_disable (ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2);
}
}
@ -641,30 +823,121 @@ void ESC_stopinput (void)
*/
uint8_t ESC_startoutput (uint8_t state)
{
ESC_SMenable (2);
App.state |= APPSTATE_OUTPUT;
CC_ATOMIC_OR(App.state, APPSTATE_OUTPUT);
return state;
}
/** Unconditional stop of updating outputs by disabling Sync Manager 2.
* Set the App.state to APPSTATE_ONPUT. Call application hook APP_safeoutput
* Set the App.state to APPSTATE_INPUT. Call application hook APP_safeoutput
* letting the user to set safe state values on outputs.
*
*/
void ESC_stopoutput (void)
{
App.state &= APPSTATE_INPUT;
CC_ATOMIC_AND(App.state, APPSTATE_INPUT);
ESC_SMdisable (2);
APP_safeoutput ();
}
/** The state handler acting on ALControl Bit(0) and SyncManager Activation BIT(4)
/** The state handler acting on SyncManager Activation BIT(4)
* events in the Al Event Request register 0x220.
*
*/
void ESC_sm_act_event (void)
{
uint8_t ac, an, as, ax, ax23;
/* Have at least on Sync Manager changed */
if ((ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE) == 0)
{
/* nothing to do */
return;
}
/* Mask state request bits + Error ACK */
ac = ESCvar.ALcontrol & ESCREG_AL_STATEMASK;
as = ESCvar.ALstatus & ESCREG_AL_STATEMASK;
an = as;
if (((ac & ESCerror) || (ac == ESCinit)))
{
/* if error bit confirmed reset */
ac &= ESCREG_AL_ERRACKMASK;
an &= ESCREG_AL_ERRACKMASK;
}
/* Enter SM changed handling for all steps but Init and Boot when Mailboxes
* is up and running
*/
if ((as & ESCREG_AL_ALLBUTINITMASK) &&
((as == ESCboot) == 0) && MBXrun)
{
/* Validate Sync Managers, reading the Activation register will
* acknowledge the SyncManager Activation event making us enter
* this execution path.
*/
ax = ESC_checkmbx (as);
ax23 = ESC_checkSM23 (as);
if ((an & ESCerror) && ((ac & ESCerror) == 0))
{
/* if in error then stay there */
}
/* Have we been forced to step down to INIT we will stop mailboxes,
* update AL Status Code and exit ESC_state
*/
else if (ax == (ESCinit | ESCerror))
{
/* If we have activated Inputs and Outputs we need to disable them */
if (CC_ATOMIC_GET(App.state))
{
ESC_stopoutput ();
ESC_stopinput ();
}
/* Stop mailboxes and update ALStatus code */
ESC_stopmbx ();
ESC_ALerror (ALERR_INVALIDMBXCONFIG);
MBXrun = 0;
ESC_ALstatus (ax);
return;
}
/* Have we been forced to step down to PREOP we will stop inputs
* and outputs, update AL Status Code and exit ESC_state
*/
else if (CC_ATOMIC_GET(App.state) && (ax23 == (ESCpreop | ESCerror)))
{
ESC_stopoutput ();
ESC_stopinput ();
if (ESCvar.SMtestresult & SMRESULT_ERRSM3)
{
ESC_ALerror (ALERR_INVALIDINPUTSM);
}
else
{
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
ESC_ALstatus (ax23);
}
}
else
{
ESC_SMack (0);
ESC_SMack (1);
ESC_SMack (2);
ESC_SMack (3);
ESC_SMack (4);
ESC_SMack (5);
ESC_SMack (6);
ESC_SMack (7);
}
}
/** The state handler acting on ALControl Bit(0)
* events in the Al Event Request register 0x220.
*
*/
void ESC_state (void)
{
uint8_t ac, an, as, ax, ax23;
uint8_t handle_smchanged = 0;
uint8_t ac, an, as;
/* Do we have a state change request pending */
if (ESCvar.ALevent & ESCREG_ALEVENT_CONTROL)
@ -673,11 +946,6 @@ void ESC_state (void)
sizeof (ESCvar.ALcontrol));
ESCvar.ALcontrol = etohs (ESCvar.ALcontrol);
}
/* Have at least on Sync Manager changed */
else if (ESCvar.ALevent & ESCREG_ALEVENT_SMCHANGE)
{
handle_smchanged = 1;
}
else
{
/* nothing to do */
@ -693,60 +961,6 @@ void ESC_state (void)
ac &= ESCREG_AL_ERRACKMASK;
an &= ESCREG_AL_ERRACKMASK;
}
/* Enter SM changed handling for all steps but Init and Boot when Mailboxes
* is up and running
*/
if (handle_smchanged && (as & ESCREG_AL_ALLBUTINITMASK) &&
!(as == ESCboot) && MBXrun)
{
/* Validate Sync Managers, reading the Activation register will
* acknowledge the SyncManager Activation event making us enter
* this execution path.
*/
ax = ESC_checkmbx (as);
ax23 = ESC_checkSM23 (as);
if ((an & ESCerror) && !(ac & ESCerror))
{
/* if in error then stay there */
return;
}
/* Have we been forced to step down to INIT we will stop mailboxes,
* update AL Status Code and exit ESC_state
*/
if (ax == (ESCinit | ESCerror))
{
/* If we have activated Inputs and Outputs we need to disable them */
if (App.state)
{
ESC_stopoutput ();
ESC_stopinput ();
}
/* Stop mailboxes and update ALStatus code */
ESC_stopmbx ();
ESC_ALerror (ALERR_INVALIDMBXCONFIG);
MBXrun = 0;
ESC_ALstatus (ax);
return;
}
/* Have we been forced to step down to PREOP we will stop inputs
* and outputs, update AL Status Code and exit ESC_state
*/
if ((App.state) && (ax23 == (ESCpreop | ESCerror)))
{
ESC_stopoutput ();
ESC_stopinput ();
if (ESCvar.SMtestresult & SMRESULT_ERRSM3)
{
ESC_ALerror (ALERR_INVALIDINPUTSM);
}
else
{
ESC_ALerror (ALERR_INVALIDOUTPUTSM);
}
ESC_ALstatus (ax23);
return;
}
}
/* Error state not acked, leave original */
if ((an & ESCerror) && ((ac & ESCerror) == 0))
@ -778,6 +992,7 @@ void ESC_state (void)
{
/* get station address */
ESC_address ();
COE_initDefaultSyncMgrPara ();
an = ESC_startmbx (ac);
break;
}
@ -799,10 +1014,17 @@ void ESC_state (void)
case OP_TO_INIT:
{
ESC_stopoutput ();
ESC_stopinput ();
ESC_stopmbx ();
an = ESCinit;
break;
}
case SAFEOP_TO_INIT:
{
ESC_stopinput ();
ESC_stopmbx ();
an = ESCinit;
break;
}
case PREOP_TO_INIT:
{
@ -846,6 +1068,9 @@ void ESC_state (void)
case OP_TO_PREOP:
{
ESC_stopoutput ();
ESC_stopinput ();
an = ESCpreop;
break;
}
case SAFEOP_TO_PREOP:
{

115
soes/esc.h
View File

@ -13,29 +13,51 @@
#include <cc.h>
#define ESCREG_ADDRESS 0x0010
#define ESCREG_DLSTATUS 0x0110
#define ESCREG_ALCONTROL 0x0120
#define ESCREG_ALSTATUS 0x0130
#define ESCREG_ALERROR 0x0134
#define ESCREG_ALEVENT 0x0220
#define ESCREG_ALEVENT_SM_MASK 0x0310
#define ESCREG_ALEVENT_SMCHANGE 0x0010
#define ESCREG_ALEVENT_CONTROL 0x0001
#define ESCREG_ALEVENT_SM2 0x0400
#define ESCREG_ALEVENT_SM3 0x0800
#define ESCREG_WDSTATUS 0x0440
#define ESCREG_SM0 0x0800
#define ESCREG_SM0STATUS (ESCREG_SM0 + 5)
#define ESCREG_SM0PDI (ESCREG_SM0 + 7)
#define ESCREG_SM1 (ESCREG_SM0 + 0x08)
#define ESCREG_SM2 (ESCREG_SM0 + 0x10)
#define ESCREG_SM3 (ESCREG_SM0 + 0x18)
#define ESCREG_LOCALTIME 0x0910
#define ESCREG_SMENABLE_BIT 0x01
#define ESCREG_AL_STATEMASK 0x001f
#define ESCREG_AL_ALLBUTINITMASK 0x0e
#define ESCREG_AL_ERRACKMASK 0x0f
#define ESCREG_ADDRESS 0x0010
#define ESCREG_DLSTATUS 0x0110
#define ESCREG_ALCONTROL 0x0120
#define ESCREG_ALSTATUS 0x0130
#define ESCREG_ALERROR 0x0134
#define ESCREG_ALEVENTMASK 0x0204
#define ESCREG_ALEVENT 0x0220
#define ESCREG_ALEVENT_SM_MASK 0x0310
#define ESCREG_ALEVENT_SMCHANGE 0x0010
#define ESCREG_ALEVENT_CONTROL 0x0001
#define ESCREG_ALEVENT_DC_LATCH 0x0002
#define ESCREG_ALEVENT_DC_SYNC0 0x0004
#define ESCREG_ALEVENT_DC_SYNC1 0x0008
#define ESCREG_ALEVENT_EEP 0x0020
#define ESCREG_ALEVENT_SM0 0x0100
#define ESCREG_ALEVENT_SM1 0x0200
#define ESCREG_ALEVENT_SM2 0x0400
#define ESCREG_ALEVENT_SM3 0x0800
#define ESCREG_WDSTATUS 0x0440
#define ESCREG_EECONTSTAT 0x0502
#define ESCREG_EEDATA 0x0508
#define ESCREG_SM0 0x0800
#define ESCREG_SM0STATUS (ESCREG_SM0 + 5)
#define ESCREG_SM0PDI (ESCREG_SM0 + 7)
#define ESCREG_SM1 (ESCREG_SM0 + 0x08)
#define ESCREG_SM2 (ESCREG_SM0 + 0x10)
#define ESCREG_SM3 (ESCREG_SM0 + 0x18)
#define ESCREG_LOCALTIME 0x0910
#define ESCREG_SYNC_ACT 0x0981
#define ESCREG_SYNC_ACT_ACTIVATED 0x01
#define ESCREG_SYNC_SYNC0_EN 0x02
#define ESCREG_SYNC_SYNC1_EN 0x04
#define ESCREG_SYNC_AUTO_ACTIVATED 0x08
#define ESCREG_SYNC0_CYCLE_TIME 0x09A0
#define ESCREG_SYNC1_CYCLE_TIME 0x09A4
#define ESCREG_SMENABLE_BIT 0x01
#define ESCREG_AL_STATEMASK 0x001f
#define ESCREG_AL_ALLBUTINITMASK 0x0e
#define ESCREG_AL_ERRACKMASK 0x0f
#define SYNCTYPE_SUPPORT_FREERUN 0x01
#define SYNCTYPE_SUPPORT_SYNCHRON 0x02
#define SYNCTYPE_SUPPORT_DCSYNC0 0x04
#define SYNCTYPE_SUPPORT_DCSYNC1 0x08
#define SYNCTYPE_SUPPORT_SUBCYCLE 0x10
#define ESCinit 0x01
#define ESCpreop 0x02
@ -74,12 +96,16 @@
#define ALERR_INVALIDSTATECHANGE 0x0011
#define ALERR_UNKNOWNSTATE 0x0012
#define ALERR_BOOTNOTSUPPORTED 0x0013
#define ALERR_NOVALIDFIRMWARE 0x0014
#define ALERR_INVALIDBOOTMBXCONFIG 0x0015
#define ALERR_INVALIDMBXCONFIG 0x0016
#define ALERR_INVALIDSMCONFIG 0x0017
#define ALERR_SYNCERROR 0x001A
#define ALERR_WATCHDOG 0x001B
#define ALERR_INVALIDOUTPUTSM 0x001D
#define ALERR_INVALIDINPUTSM 0x001E
#define ALERR_DCINVALIDSYNCCFG 0x0030
#define ALERR_DCSYNC0CYCLETIME 0x0036
#define MBXERR_SYNTAX 0x0001
#define MBXERR_UNSUPPORTEDPROTOCOL 0x0002
@ -488,6 +514,10 @@ extern uint8_t ESC_MBX1_smc;
void ESC_config (esc_cfg_t * cfg);
void ESC_ALerror (uint16_t errornumber);
void ESC_ALeventwrite (uint32_t event);
uint32_t ESC_ALeventread (void);
void ESC_ALeventmaskwrite (uint32_t mask);
uint32_t ESC_ALeventmaskread (void);
void ESC_ALstatus (uint8_t status);
void ESC_SMstatus (uint8_t n);
uint8_t ESC_WDstatus (void);
@ -502,6 +532,7 @@ void ESC_stopinput (void);
uint8_t ESC_startoutput (uint8_t state);
void ESC_stopoutput (void);
void ESC_state (void);
void ESC_sm_act_event (void);
/* From hardware file */
void ESC_read (uint16_t address, void *buf, uint16_t len);
@ -516,6 +547,11 @@ extern uint8_t MBX[];
extern _MBXcontrol MBXcontrol[];
extern uint8_t MBXrun;
extern uint16_t ESC_SM2_sml, ESC_SM3_sml;
extern uint8_t dc_sync;
extern int8_t sync_counter;
extern uint16_t sync_counter_limit;
extern uint16_t TXPDOsize;
extern uint16_t RXPDOsize;
typedef struct
{
@ -528,4 +564,37 @@ typedef struct
extern _App App;
void DIG_process (uint8_t flags);
#define DIG_PROCESS_INPUTS_FLAG 0x01
#define DIG_PROCESS_OUTPUTS_FLAG 0x02
#define DIG_PROCESS_WD_FLAG 0x04
#define DIG_PROCESS_APP_HOOK_FLAG 0x08
/* ATOMIC operations are used when running interrupt driven */
#ifndef CC_ATOMIC_SET
#define CC_ATOMIC_SET(var,val) (var = val)
#endif
#ifndef CC_ATOMIC_GET
#define CC_ATOMIC_GET(var) (var)
#endif
#ifndef CC_ATOMIC_ADD
#define CC_ATOMIC_ADD(var,val) (var += val)
#endif
#ifndef CC_ATOMIC_SUB
#define CC_ATOMIC_SUB(var,val) (var -= val)
#endif
#ifndef CC_ATOMIC_AND
#define CC_ATOMIC_AND(var,val) (var &= val)
#endif
#ifndef CC_ATOMIC_OR
#define CC_ATOMIC_OR(var,val) (var |= val)
#endif
#endif

118
soes/esc_coe.c
View File

@ -61,6 +61,122 @@ int32_t SDO_findobject (uint16_t index)
return n;
}
/** Get the value for requested SDO 0x1C32 or 0x1C33 sub index
*
* @param[in] index = value on index of object we want to locate
* @param[in] subindex = value on subindex of object we want to locate
* @param[out] buf = buf to copy value to
* @param[in] datatype = EtherCAT datatype of buf
* @return 1 if value was found, else 0.
*/
int COE_getSyncMgrPara (uint16_t index, uint8_t subindex, void * buf, uint16_t datatype)
{
int result = 0;
int32_t nidx;
int32_t snidx;
const _objd *objd;
nidx = SDO_findobject(index);
if(nidx < 0)
{
return result;
}
else if((index != 0x1c32) && (index != 0x1c33) && (index != 0x10F1))
{
return result;
}
snidx = SDO_findsubindex(nidx, subindex);
if(snidx >= 0)
{
objd = SDOobjects[nidx].objdesc;
if((objd[snidx].data != NULL) &&
(objd[snidx].datatype == datatype))
{
memcpy(buf, objd[snidx].data, objd[snidx].bitlength / 8 );
result = 1;
}
else
{
if((datatype == DTYPE_UNSIGNED32) &&
(objd[snidx].datatype == datatype))
{
*(uint32_t *)buf = objd[snidx].value;
result = 1;
}
else if((datatype == DTYPE_UNSIGNED16) &&
(objd[snidx].datatype == datatype))
{
*(uint16_t *)buf = (uint16_t)objd[snidx].value;
result = 1;
}
else if((datatype == DTYPE_UNSIGNED8) &&
(objd[snidx].datatype == datatype))
{
*(uint8_t *)buf = (uint8_t)objd[snidx].value;
result = 1;
}
}
}
return result;
}
/** Init default values for SDO Sync Objects
*
*/
void COE_initDefaultSyncMgrPara (void)
{
uint32_t i,j;
const _objd *objd;
int32_t n = 0;
/* 1C3x */
for(i = 0x1C32; i <= 0x1C33; i ++)
{
/* Look if index is present */
n = SDO_findobject(i);
if(n < 0)
{
continue;
}
/* Load default values */
objd = SDOobjects[n].objdesc;
for(j = 1; j <= SDOobjects[n].maxsub; j++ )
{
if(objd[j].data != NULL)
{
*(uint32_t *)objd[j].data = objd[j].value;
}
if(objd[j].subindex >= SDOobjects[n].maxsub)
{
break;
}
}
}
/* Look if index is present */
n = SDO_findobject(0x10F1);
if(n >= 0)
{
/* Load default values */
objd = SDOobjects[n].objdesc;
for(j = 1; j <= objd[0].value; j++ )
{
if(objd[j].data != NULL)
{
*(uint32_t *)objd[j].data = objd[j].value;
}
}
}
}
/** Calculate the size in Bytes of RxPDO or TxPDOs by adding the objects
* in SyncManager
* SDO 1C1x.
@ -443,6 +559,8 @@ void SDO_infoerror (uint32_t abortcode)
coeres->infoheader.fragmentsleft = 0;
coeres->index = htoel (abortcode);
MBXcontrol[MBXout].state = MBXstate_outreq;
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
}

2
soes/esc_coe.h
View File

@ -83,6 +83,8 @@ typedef struct CC_PACKED
void ESC_coeprocess (void);
uint16_t sizeOfPDO (uint16_t index);
void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode);
void COE_initDefaultSyncMgrPara (void);
int COE_getSyncMgrPara (uint16_t index, uint8_t subindex, void * buf, uint16_t datatype);
extern void ESC_objecthandler (uint16_t index, uint8_t subindex);
extern int ESC_pre_objecthandler (uint16_t index, uint8_t subindex);