#include "TestIGH.h"
/*****************************************************************************/
void check_domain_state(void)
{
ec_domain_state_t ds;
ecrt_domain_state(domain, &ds);
if (ds.working_counter != domain_state.working_counter) {
printf("Domain: Working Counter changed to %u.\n",
ds.working_counter);
}
if (ds.wc_state != domain_state.wc_state) {
printf("Domain: Working Counter State changed to %u.\n", ds.wc_state);
}
domain_state = ds;
}
/****************************************************************************/
struct period_info {
struct timespec next_period;
long period_ns;
};
/****************************************************************************/
static void inc_period(struct period_info *pinfo)
{
pinfo->next_period.tv_nsec += pinfo->period_ns;
while (pinfo->next_period.tv_nsec >= 1000000000) {
/* timespec nsec overflow */
pinfo->next_period.tv_sec++;
pinfo->next_period.tv_nsec -= 1000000000;
}
}
/****************************************************************************/
static void periodic_task_init(struct period_info *pinfo)
{
/* for simplicity, hardcoding a 100 ms period */
pinfo->period_ns = 100000000;
/* find timestamp to first run */
clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period));
}
/****************************************************************************/
static void do_rt_task()
{
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain);
// check process data state
check_domain_state();
// read process data
int16_t raw_value = EC_READ_S16(domain_pd + off_ana_in);
cycle_counter++;
if (cycle_counter % 10 == 0) {
printf("AnaIn: Value %5i ~ %.3f V\n",
raw_value, raw_value * 10.0 / 32768.0);
}
// send process data
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
/****************************************************************************/
static void wait_rest_of_period(struct period_info *pinfo)
{
inc_period(pinfo);
/* for simplicity, ignoring possibilities of signal wakes */
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
&pinfo->next_period, NULL);
}
/****************************************************************************/
void *simple_cyclic_task(void *data)
{
printf("Started cyclic thread.\n");
struct period_info pinfo;
periodic_task_init(&pinfo);
while (1) {
do_rt_task();
wait_rest_of_period(&pinfo);
}
return NULL;
}
/****************************************************************************/
int init_ethercat()
{
ec_slave_config_t *sc;
master = ecrt_request_master(0);
if (!master) {
return -1;
}
domain = ecrt_master_create_domain(master);
if (!domain) {
return -1;
}
if (!(sc = ecrt_master_slave_config(master,
AnaInSlavePos, Beckhoff_EL3104))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc, EC_END, el3104_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
// Register PDOs for exchange
if (ecrt_domain_reg_pdo_entry_list(domain, domain_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
if (!(domain_pd = ecrt_domain_data(domain))) {
return -1;
}
return 0;
}
/****************************************************************************/
int main(int argc, char* argv[])
{
struct sched_param param;
pthread_attr_t attr;
pthread_t thread;
int ret;
ret = init_ethercat();
if (ret) {
printf("Failed to init EtherCAT master.\n");
exit(-2);
}
/* Lock memory */
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
printf("mlockall() failed: %m\n");
exit(-2);
}
/* Initialize pthread attributes (default values) */
ret = pthread_attr_init(&attr);
if (ret) {
printf("init pthread attributes failed\n");
goto out;
}
/* Set a specific stack size */
ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN + MY_STACK_SIZE);
if (ret) {
printf("pthread setstacksize failed\n");
goto out;
}
/* Set scheduler policy and priority of pthread */
ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
if (ret) {
printf("pthread setschedpolicy failed\n");
goto out;
}
param.sched_priority = 80;
ret = pthread_attr_setschedparam(&attr, ¶m);
if (ret) {
printf("pthread setschedparam failed\n");
goto out;
}
/* Use scheduling parameters of attr */
ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (ret) {
printf("pthread setinheritsched failed\n");
goto out;
}
/* Create a pthread with specified attributes */
ret = pthread_create(&thread, &attr, simple_cyclic_task, NULL);
if (ret) {
printf("create pthread failed: %s\n", strerror(ret));
goto out;
}
ret = pthread_setname_np(thread, "ethercat-rt");
if (ret) {
printf("failed to set thread name\n");
}
/* Join the thread and wait until it is done */
ret = pthread_join(thread, NULL);
if (ret) {
printf("join pthread failed: %m\n");
}
out:
return ret;
}
/****************************************************************************/
