519 lines
15 KiB
C
519 lines
15 KiB
C
/** \file
|
|
* \brief Example code for Simple Open EtherCAT master
|
|
*
|
|
* This is a minimal example running rt-kernel TTOS. Check
|
|
* tutorial files in documentations for som steps to get your
|
|
* system running.
|
|
* (c)Andreas Karlsson 2012
|
|
*/
|
|
|
|
#include <kern.h>
|
|
#include "ethercat.h"
|
|
#include "string.h"
|
|
#include <oshw.h>
|
|
#include <config.h>
|
|
|
|
#include <defBF537.h>
|
|
|
|
#define pPORTFIO_SET ((vuint16_t *)PORTFIO_SET)
|
|
#define pPORTFIO_CLEAR ((vuint16_t *)PORTFIO_CLEAR)
|
|
#define pPORTFIO_DIR ((vuint16_t *)PORTFIO_DIR)
|
|
|
|
#define EK1100_1 1
|
|
#define EL4001_1 2
|
|
#define EL3061_1 3
|
|
#define EL1008_1 4
|
|
#define EL1008_2 5
|
|
#define EL2622_1 6
|
|
#define EL2622_2 7
|
|
#define EL2622_3 8
|
|
#define EL2622_4 9
|
|
#define NUMBER_OF_SLAVES 9
|
|
|
|
|
|
typedef struct
|
|
{
|
|
uint8 in1;
|
|
uint8 in2;
|
|
uint8 in3;
|
|
uint8 in4;
|
|
uint8 in5;
|
|
uint8 in6;
|
|
uint8 in7;
|
|
uint8 in8;
|
|
} in_EL1008_t;
|
|
|
|
typedef struct
|
|
{
|
|
uint8 out1;
|
|
uint8 out2;
|
|
} out_EL2622_t;
|
|
|
|
typedef struct
|
|
{
|
|
int16 out1;
|
|
} out_EL4001_t;
|
|
|
|
typedef struct
|
|
{
|
|
int32 in1;
|
|
} in_EL3061_t;
|
|
|
|
out_EL4001_t slave_EL4001_1;
|
|
in_EL3061_t slave_EL3061_1;
|
|
in_EL1008_t slave_EL1008_1;
|
|
in_EL1008_t slave_EL1008_2;
|
|
out_EL2622_t slave_EL2622_1;
|
|
out_EL2622_t slave_EL2622_2;
|
|
out_EL2622_t slave_EL2622_3;
|
|
|
|
uint32 network_configuration(void)
|
|
{
|
|
/* Do we got expected number of slaves from config */
|
|
if (ec_slavecount < NUMBER_OF_SLAVES)
|
|
return 0;
|
|
|
|
/* Verify slave by slave that it is correct*/
|
|
if (strcmp(ec_slave[EK1100_1].name,"EK1100"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL4001_1].name,"EL4001"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL3061_1].name,"EL3061"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL1008_1].name,"EL1008"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL1008_2].name,"EL1008"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL2622_1].name,"EL2622"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL2622_2].name,"EL2622"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL2622_3].name,"EL2622"))
|
|
return 0;
|
|
else if (strcmp(ec_slave[EL2622_4].name,"EL2622"))
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
int32 get_input_int32(uint16 slave_no,uint8 module_index)
|
|
{
|
|
int32 return_value;
|
|
uint8 *data_ptr;
|
|
/* Get the IO map pointer from the ec_slave struct */
|
|
data_ptr = ec_slave[slave_no].inputs;
|
|
/* Move pointer to correct module index */
|
|
data_ptr += module_index * 4;
|
|
/* Read value byte by byte since all targets can't handle misaligned
|
|
* addresses
|
|
*/
|
|
return_value = *data_ptr++;
|
|
return_value += (*data_ptr++ << 8);
|
|
return_value += (*data_ptr++ << 16);
|
|
return_value += (*data_ptr++ << 24);
|
|
|
|
return return_value;
|
|
}
|
|
|
|
void set_input_int32 (uint16 slave_no, uint8 module_index, int32 value)
|
|
{
|
|
uint8 *data_ptr;
|
|
/* Get the IO map pointer from the ec_slave struct */
|
|
data_ptr = ec_slave[slave_no].inputs;
|
|
/* Move pointer to correct module index */
|
|
data_ptr += module_index * 4;
|
|
/* Read value byte by byte since all targets can handle misaligned
|
|
* addresses
|
|
*/
|
|
*data_ptr++ = (value >> 0) & 0xFF;
|
|
*data_ptr++ = (value >> 8) & 0xFF;
|
|
*data_ptr++ = (value >> 16) & 0xFF;
|
|
*data_ptr++ = (value >> 24) & 0xFF;
|
|
}
|
|
|
|
uint8 get_input_bit (uint16 slave_no,uint8 module_index)
|
|
{
|
|
/* Get the the startbit position in slaves IO byte */
|
|
uint8 startbit = ec_slave[slave_no].Istartbit;
|
|
/* Mask bit and return boolean 0 or 1 */
|
|
if (*ec_slave[slave_no].inputs & BIT (module_index - 1 + startbit))
|
|
return 1;
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
int16 get_output_int16(uint16 slave_no,uint8 module_index)
|
|
{
|
|
int16 return_value;
|
|
uint8 *data_ptr;
|
|
|
|
/* Get the IO map pointer from the ec_slave struct */
|
|
data_ptr = ec_slave[slave_no].outputs;
|
|
/* Move pointer to correct module index */
|
|
data_ptr += module_index * 2;
|
|
/* Read value byte by byte since all targets can handle misaligned
|
|
* addresses
|
|
*/
|
|
return_value = *data_ptr++;
|
|
return_value += (*data_ptr++ << 8);
|
|
|
|
return return_value;
|
|
}
|
|
|
|
void set_output_int16 (uint16 slave_no, uint8 module_index, int16 value)
|
|
{
|
|
uint8 *data_ptr;
|
|
/* Get the IO map pointer from the ec_slave struct */
|
|
data_ptr = ec_slave[slave_no].outputs;
|
|
/* Move pointer to correct module index */
|
|
data_ptr += module_index * 2;
|
|
/* Read value byte by byte since all targets can handle misaligned
|
|
* addresses
|
|
*/
|
|
*data_ptr++ = (value >> 0) & 0xFF;
|
|
*data_ptr++ = (value >> 8) & 0xFF;
|
|
}
|
|
|
|
uint8 get_output_bit (uint16 slave_no,uint8 module_index)
|
|
{
|
|
/* Get the the startbit position in slaves IO byte */
|
|
uint8 startbit = ec_slave[slave_no].Ostartbit;
|
|
/* Mask bit and return boolean 0 or 1 */
|
|
if (*ec_slave[slave_no].outputs & BIT (module_index - 1 + startbit))
|
|
return 1;
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
void set_output_bit (uint16 slave_no, uint8 module_index, uint8 value)
|
|
{
|
|
/* Get the the startbit position in slaves IO byte */
|
|
uint8 startbit = ec_slave[slave_no].Ostartbit;
|
|
/* Set or Clear bit */
|
|
if (value == 0)
|
|
*ec_slave[slave_no].outputs &= ~(1 << (module_index - 1 + startbit));
|
|
else
|
|
*ec_slave[slave_no].outputs |= (1 << (module_index - 1 + startbit));
|
|
}
|
|
|
|
|
|
extern tt_sched_t * tt_sched[];
|
|
|
|
char IOmap[128];
|
|
int dorun = 0;
|
|
|
|
uint8_t load1s, load5s, load10s;
|
|
uint32_t error_counter = 0;
|
|
|
|
void tt_error (uint32_t task_ix);
|
|
void tt_error (uint32_t task_ix)
|
|
{
|
|
error_counter++;
|
|
}
|
|
|
|
static void my_cyclic_callback (void * arg)
|
|
{
|
|
while (1)
|
|
{
|
|
stats_get_load (&load1s, &load5s, &load10s);
|
|
rprintp ("Processor load was %d:%d:%d (1s:5s:10s) with TT errors: %d\n",
|
|
load1s, load5s, load10s,error_counter);
|
|
task_delay(20000);
|
|
}
|
|
}
|
|
|
|
|
|
void read_io (void * arg)
|
|
{
|
|
/* Function connected to cyclic TTOS task
|
|
* The function is executed cyclic according
|
|
* sceduel specified in schedule.tt
|
|
*/
|
|
*pPORTFIO_SET = BIT (6);
|
|
/* Send and receive processdata
|
|
* Note that you need som synchronization
|
|
* case you modifey IO in local application
|
|
*/
|
|
ec_send_processdata();
|
|
ec_receive_processdata(EC_TIMEOUTRET);
|
|
*pPORTFIO_CLEAR = BIT (6);
|
|
}
|
|
|
|
|
|
void simpletest(void *arg)
|
|
{
|
|
|
|
char *ifname = arg;
|
|
int cnt, i, j;
|
|
|
|
*pPORTFIO_DIR |= BIT (6);
|
|
|
|
rprintp("Starting simple test\n");
|
|
|
|
/* initialise SOEM */
|
|
if (ec_init(ifname))
|
|
{
|
|
rprintp("ec_init succeeded.\n");
|
|
|
|
/* find and auto-config slaves */
|
|
if ( ec_config_init(FALSE) > 0 )
|
|
{
|
|
rprintp("%d slaves found and configured.\n",ec_slavecount);
|
|
|
|
/* Check network setup */
|
|
if (network_configuration())
|
|
{
|
|
/* Run IO mapping */
|
|
ec_config_map(&IOmap);
|
|
|
|
rprintp("Slaves mapped, state to SAFE_OP.\n");
|
|
/* wait for all slaves to reach SAFE_OP state */
|
|
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
|
|
|
|
/* Print som information on the mapped network */
|
|
for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
|
|
{
|
|
rprintp("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
|
|
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
|
|
ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
|
|
rprintp(" Configured address: %x\n", ec_slave[cnt].configadr);
|
|
rprintp(" Outputs address: %x\n", ec_slave[cnt].outputs);
|
|
rprintp(" Inputs address: %x\n", ec_slave[cnt].inputs);
|
|
|
|
for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
|
|
{
|
|
rprintp(" FMMU%1d Ls:%x Ll:%4d Lsb:%d Leb:%d Ps:%x Psb:%d Ty:%x Act:%x\n", j,
|
|
(int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
|
|
ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
|
|
ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
|
|
}
|
|
rprintp(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
|
|
ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
|
|
|
|
}
|
|
|
|
rprintp("Request operational state for all slaves\n");
|
|
ec_slave[0].state = EC_STATE_OPERATIONAL;
|
|
/* send one valid process data to make outputs in slaves happy*/
|
|
ec_send_processdata();
|
|
ec_receive_processdata(EC_TIMEOUTRET);
|
|
/* request OP state for all slaves */
|
|
ec_writestate(0);
|
|
/* wait for all slaves to reach OP state */
|
|
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
|
|
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
|
|
{
|
|
rprintp("Operational state reached for all slaves.\n");
|
|
}
|
|
else
|
|
{
|
|
rprintp("Not all slaves reached operational state.\n");
|
|
ec_readstate();
|
|
for(i = 1; i<=ec_slavecount ; i++)
|
|
{
|
|
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
|
|
{
|
|
rprintp("Slave %d State=0x%04x StatusCode=0x%04x\n",
|
|
i, ec_slave[i].state, ec_slave[i].ALstatuscode);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/* Simple blinking lamps BOX demo */
|
|
uint8 digout = 0;
|
|
|
|
slave_EL4001_1.out1 = (int16)0x3FFF;
|
|
set_output_int16(EL4001_1,0,slave_EL4001_1.out1);
|
|
|
|
task_spawn ("t_StatsPrint", my_cyclic_callback, 20, 1024, (void *)NULL);
|
|
tt_start_wait (tt_sched[0]);
|
|
|
|
while(1)
|
|
{
|
|
dorun = 0;
|
|
slave_EL1008_1.in1 = get_input_bit(EL1008_1,1); // Start button
|
|
slave_EL1008_1.in2 = get_input_bit(EL1008_1,2); // Turnkey RIGHT
|
|
slave_EL1008_1.in3 = get_input_bit(EL1008_1,3); // Turnkey LEFT
|
|
|
|
/* (Turnkey MIDDLE + Start button) OR Turnkey RIGHT OR Turnkey LEFT
|
|
Turnkey LEFT: Light positions bottom to top. Loop, slow operation.
|
|
Turnkey MIDDLE: Press start button to light positions bottom to top. No loop, fast operation.
|
|
Turnkey RIGHT: Light positions bottom to top. Loop, fast operation.
|
|
*/
|
|
if (slave_EL1008_1.in1 || slave_EL1008_1.in2 || slave_EL1008_1.in3)
|
|
{
|
|
digout = 0;
|
|
/* *ec_slave[6].outputs = digout; */
|
|
/* set_output_bit(slave_name #,index as 1 output on module , value */
|
|
set_output_bit(EL2622_1,1,(digout & BIT (0))); /* Start button */
|
|
set_output_bit(EL2622_1,2,(digout & BIT (1))); /* Turnkey RIGHT */
|
|
set_output_bit(EL2622_2,1,(digout & BIT (2))); /* Turnkey LEFT */
|
|
set_output_bit(EL2622_2,2,(digout & BIT (3)));
|
|
set_output_bit(EL2622_3,1,(digout & BIT (4)));
|
|
set_output_bit(EL2622_3,2,(digout & BIT (5)));
|
|
|
|
while(dorun < 95)
|
|
{
|
|
dorun++;
|
|
|
|
if (slave_EL1008_1.in3)
|
|
task_delay(tick_from_ms(20));
|
|
else
|
|
task_delay(tick_from_ms(5));
|
|
|
|
digout = (uint8) (digout | BIT((dorun / 16) & 0xFF));
|
|
|
|
set_output_bit(EL2622_1,1,(digout & BIT (0))); /* LED1 */
|
|
set_output_bit(EL2622_1,2,(digout & BIT (1))); /* LED2 */
|
|
set_output_bit(EL2622_2,1,(digout & BIT (2))); /* LED3 */
|
|
set_output_bit(EL2622_2,2,(digout & BIT (3))); /* LED4 */
|
|
set_output_bit(EL2622_3,1,(digout & BIT (4))); /* LED5 */
|
|
set_output_bit(EL2622_3,2,(digout & BIT (5))); /* LED6 */
|
|
|
|
slave_EL1008_1.in1 = get_input_bit(EL1008_1,2); /* Turnkey RIGHT */
|
|
slave_EL1008_1.in2 = get_input_bit(EL1008_1,3); /* Turnkey LEFT */
|
|
slave_EL3061_1.in1 = get_input_int32(EL3061_1,0); /* Read AI */
|
|
}
|
|
}
|
|
task_delay(tick_from_ms(2));
|
|
|
|
}
|
|
}
|
|
else
|
|
{
|
|
rprintp("Mismatch of network units!\n");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
rprintp("No slaves found!\n");
|
|
}
|
|
rprintp("End simple test, close socket\n");
|
|
/* stop SOEM, close socket */
|
|
ec_close();
|
|
}
|
|
else
|
|
{
|
|
rprintp("ec_init failed");
|
|
}
|
|
rprintp("End program\n");
|
|
}
|
|
|
|
static void test_osal_timer_timeout_us (const uint32 timeout_us)
|
|
{
|
|
osal_timert timer;
|
|
|
|
ASSERT (timeout_us > 4000);
|
|
|
|
osal_timer_start (&timer, timeout_us);
|
|
ASSERT (osal_timer_is_expired (&timer) == false);
|
|
osal_usleep (timeout_us - 2000);
|
|
ASSERT (osal_timer_is_expired (&timer) == false);
|
|
osal_usleep (4000);
|
|
ASSERT (osal_timer_is_expired (&timer));
|
|
}
|
|
|
|
static void test_osal_timer (void)
|
|
{
|
|
test_osal_timer_timeout_us (10*1000); /* 10ms */
|
|
test_osal_timer_timeout_us (100*1000); /* 100ms */
|
|
test_osal_timer_timeout_us (1000*1000); /* 1s */
|
|
test_osal_timer_timeout_us (2000*1000); /* 2s */
|
|
}
|
|
|
|
#define USECS_PER_SEC 1000000
|
|
#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND)
|
|
#ifndef ABS
|
|
#define ABS(x) ((x) < 0 ? -(x) : (x))
|
|
#endif
|
|
|
|
static int32 time_difference_us (const ec_timet stop, const ec_timet start)
|
|
{
|
|
int32 difference_us;
|
|
|
|
ASSERT (stop.sec >= start.sec);
|
|
if (stop.sec == start.sec)
|
|
{
|
|
ASSERT (stop.usec >= start.usec);
|
|
}
|
|
|
|
difference_us = (stop.sec - start.sec) * USECS_PER_SEC;
|
|
difference_us += ((int32)stop.usec - (int32)start.usec);
|
|
|
|
ASSERT (difference_us >= 0);
|
|
return difference_us;
|
|
}
|
|
|
|
/**
|
|
* Test osal_current_time() by using it for measuring how long an osal_usleep()
|
|
* takes, in specified number of microseconds.
|
|
*/
|
|
static void test_osal_current_time_for_delay_us (const int32 sleep_time_us)
|
|
{
|
|
ec_timet start;
|
|
ec_timet stop;
|
|
int32 measurement_us;
|
|
int32 deviation_us;
|
|
const int32 usleep_accuracy_us = USECS_PER_TICK;
|
|
boolean is_deviation_within_tolerance;
|
|
|
|
start = osal_current_time ();
|
|
osal_usleep (sleep_time_us);
|
|
stop = osal_current_time ();
|
|
|
|
measurement_us = time_difference_us (stop, start);
|
|
deviation_us = ABS (measurement_us - sleep_time_us);
|
|
is_deviation_within_tolerance = deviation_us <= usleep_accuracy_us;
|
|
ASSERT (is_deviation_within_tolerance);
|
|
}
|
|
|
|
static void test_osal_current_time (void)
|
|
{
|
|
test_osal_current_time_for_delay_us (0);
|
|
test_osal_current_time_for_delay_us (1);
|
|
test_osal_current_time_for_delay_us (500);
|
|
test_osal_current_time_for_delay_us (999);
|
|
test_osal_current_time_for_delay_us (USECS_PER_TICK);
|
|
test_osal_current_time_for_delay_us (USECS_PER_TICK-1);
|
|
test_osal_current_time_for_delay_us (USECS_PER_TICK+1);
|
|
test_osal_current_time_for_delay_us (2 * 1000 * 1000); /* 2s */
|
|
}
|
|
|
|
#include <lwip/inet.h>
|
|
|
|
static void test_oshw_htons (void)
|
|
{
|
|
uint16 network;
|
|
uint16 host;
|
|
|
|
host = 0x1234;
|
|
network = oshw_htons (host);
|
|
ASSERT (network == htons (host));
|
|
}
|
|
|
|
static void test_oshw_ntohs (void)
|
|
{
|
|
uint16 host;
|
|
uint16 network;
|
|
|
|
network = 0x1234;
|
|
host = oshw_ntohs (network);
|
|
ASSERT (host == ntohs (network));
|
|
}
|
|
|
|
int main (void)
|
|
{
|
|
test_oshw_htons ();
|
|
test_oshw_ntohs ();
|
|
test_osal_timer ();
|
|
test_osal_current_time ();
|
|
|
|
rprintp("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
|
|
|
|
task_spawn ("simpletest", simpletest, 9, 8192, NULL);
|
|
|
|
return (0);
|
|
}
|
|
|