/** Mandatory: Hook called from the slave stack SDO Download handler to act on
* user specified Index and Sub-index.
*
* @param[in] index = index of SDO download request to handle
* @param[in] sub-index = sub-index of SDO download request to handle
*/
void ESC_objecthandler (uint16_t index, uint8_t subindex)
{
switch (index)
{
case 0x1c12:
{
if (rxpdoitems > 1)
{
rxpdoitems = 1;
}
if ((rxpdomap != 0x1600) && (rxpdomap != 0x1601)
&& (rxpdomap != 0x0000))
{
rxpdomap = 0x1600;
}
RXPDOsize = SM2_sml = sizeRXPDO ();
break;
}
case 0x1c13:
{
if (txpdoitems > 1)
{
txpdoitems = 1;
}
if ((txpdomap != 0x1A00) && (txpdomap != 0x1A01)
&& (rxpdomap != 0x0000))
{
txpdomap = 0x1A00;
}
TXPDOsize = SM3_sml = sizeTXPDO ();
break;
}
case 0x7100:
{
switch (subindex)
{
case 0x01:
{
encoder_scale_mirror = encoder_scale;
break;
}
}
break;
}
case 0x8001:
{
switch (subindex)
{
case 0x01:
{
Cb.reset_counter = 0;
break;
}
}
break;
}
}
}
/** SOES main loop. Start by initializing the stack software followed by
* the application loop for cyclic read the EtherCAT state and staus, update
* of I/O.
*/
void soes (void *arg)
{
DPRINT ("SOES (Simple Open EtherCAT Slave)\n");
TXPDOsize = SM3_sml = sizeTXPDO ();
RXPDOsize = SM2_sml = sizeRXPDO ();
/* Setup post config hooks */
static esc_cfg_t config =
{
.pre_state_change_hook = NULL,
.post_state_change_hook = post_state_change_hook
};
ESC_config ((esc_cfg_t *)&config);
ESC_reset();
ESC_init (spi_name);
task_delay (tick_from_ms (200));
/* 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);
}
/* Pre FoE to set up Application information */
bootstrap_foe_init ();
/* Init FoE */
FOE_init();
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
DPRINT ("Application_loop GO\n");
/* application run loop */
while (1)
{
/* On init restore PDO mappings to default size */
if((ESCvar.ALstatus & 0x0f) == ESCinit)
{
txpdomap = DEFAULTTXPDOMAP;
rxpdomap = DEFAULTRXPDOMAP;
txpdoitems = DEFAULTTXPDOITEMS;
rxpdoitems = DEFAULTTXPDOITEMS;
}
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
/* Check the state machine */
ESC_state ();
/* If else to two separate execution paths
* If we're running BOOSTRAP
* - MailBox
* - FoE
* Else we're running normal execution
* - MailBox
* - CoE
*/
if(local_boot_state)
{
if (ESC_mbxprocess ())
{
ESC_foeprocess ();
ESC_xoeprocess ();
}
bootstrap_state ();
}
else
{
if (ESC_mbxprocess ())
{
ESC_coeprocess ();
ESC_xoeprocess ();
}
DIG_process ();
}
};
}
/** The state handler acting on ALControl Bit(0) and SyncManager Activation BIT(4)
* events in the Al Event Request register 0x220.
*
*/
void ESC_state (void)
{
uint8_t ac, an, as, ax, ax23;
uint8_t handle_smchanged = 0;
/* Do we have a state change request pending */
if (ESCvar.ALevent & ESCREG_ALEVENT_CONTROL)
{
ESC_read (ESCREG_ALCONTROL, (void *) &ESCvar.ALcontrol,
sizeof (ESCvar.ALcontrol), (void *) &ESCvar.ALevent);
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 */
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 (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))
{
return;
}
/* Mask high bits ALcommand, low bits ALstatus */
as = (ac << 4) | (as & 0x0f);
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->pre_state_change_hook)
{
esc_cfg->pre_state_change_hook (&as, &an);
}
/* Switch through the state change requested via AlControl from
* current state read in AL status
*/
switch (as)
{
case INIT_TO_INIT:
case PREOP_TO_PREOP:
case OP_TO_OP:
{
break;
}
case INIT_TO_PREOP:
{
/* get station address */
ESC_address ();
an = ESC_startmbx (ac);
break;
}
case INIT_TO_BOOT:
case BOOT_TO_BOOT:
{
/* get station address */
ESC_address ();
an = ESC_startmbxboot (ac);
break;
}
case INIT_TO_SAFEOP:
case INIT_TO_OP:
{
an = ESCinit | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_INIT:
{
ESC_stopoutput ();
}
case SAFEOP_TO_INIT:
{
ESC_stopinput ();
}
case PREOP_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case BOOT_TO_INIT:
{
ESC_stopmbx ();
an = ESCinit;
break;
}
case PREOP_TO_BOOT:
case BOOT_TO_PREOP:
case BOOT_TO_SAFEOP:
case BOOT_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case PREOP_TO_SAFEOP:
case SAFEOP_TO_SAFEOP:
{
SM2_sml = sizeRXPDO ();
SM3_sml = sizeTXPDO ();
an = ESC_startinput (ac);
if (an == ac)
{
ESC_SMenable (2);
}
break;
}
case PREOP_TO_OP:
{
an = ESCpreop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case OP_TO_PREOP:
{
ESC_stopoutput ();
}
case SAFEOP_TO_PREOP:
{
ESC_stopinput ();
an = ESCpreop;
break;
}
case SAFEOP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
break;
}
case SAFEOP_TO_OP:
{
an = ESC_startoutput (ac);
break;
}
case OP_TO_BOOT:
{
an = ESCsafeop | ESCerror;
ESC_ALerror (ALERR_INVALIDSTATECHANGE);
ESC_stopoutput ();
break;
}
case OP_TO_SAFEOP:
{
an = ESCsafeop;
ESC_stopoutput ();
break;
}
default:
{
if (an == ESCop)
{
ESC_stopoutput ();
an = ESCsafeop;
}
if (as == ESCsafeop)
{
ESC_stopinput ();
}
an |= ESCerror;
ESC_ALerror (ALERR_UNKNOWNSTATE);
break;
}
}
/* Call post state change hook case it have been configured */
if ((esc_cfg != NULL) && esc_cfg->post_state_change_hook)
{
esc_cfg->post_state_change_hook (&as, &an);
}
if (!(an & ESCerror) && (ESCvar.ALerror))
{
/* clear error */
ESC_ALerror (ALERR_NONE);
}
ESC_ALstatus (an);
}
