main_profile_velocity_2.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635
  1. /**
  2. * manual compile : gcc main_profile_velocity_2.c -o ectest_PV -I/opt/etherlab/include -L/opt/etherlab/lib -lethercat -Wl,--rpath -Wl,/opt/etherlab/lib
  3. */
  4. #include <stdio.h>
  5. #include <stdint.h>
  6. #include <unistd.h>
  7. #include <stdbool.h>
  8. #include <string.h>
  9. #include <signal.h>
  10. #include <errno.h>
  11. #include <sys/mman.h>
  12. #include <sys/types.h>
  13. #include <sys/resource.h>
  14. #include <time.h>
  15. #include <sched.h>
  16. #include "ecrt.h"
  17. // **********************************************************
  18. // Application parameters.
  19. #define FREQUENCY 1000
  20. #define CLOCK_TO_USE CLOCK_REALTIME
  21. #define MEASURE_TIMING
  22. #define NSEC_PER_SEC (1000000000L)
  23. #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)
  24. #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
  25. #define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
  26. #ifdef MEASURE_TIMING
  27. // Log tht save data.
  28. char logname[50];
  29. FILE *logfile = NULL;
  30. int log_item_id = 1;
  31. int num_of_timeouts = 0;
  32. #endif
  33. // quit flag.
  34. volatile bool is_quit = false;
  35. // Period.
  36. const struct timespec cycletime = {0, PERIOD_NS};
  37. // **********************************************************
  38. // Operation model.
  39. #define MOTOR_MODEL_PROFILE_VELOCITY 0x03 // Profile Velocity Mode.
  40. // Control word.
  41. #define MOTOR_MODEL_CONTROL_WORD_HALT 0x1 << 8 // Set motor halt.
  42. // Profile_Acceleration.
  43. #define MOTOR_PROFILE_ACCELERATION (10 * 1000) // 10s, unit: millisecond from 0 rpm to 3000 rpm.
  44. // Profile_Deceleration.
  45. #define MOTOR_PROFILE_DECELERATION (10 * 1000) // 10s, unit: millisecond from 0 rpm to 3000 rpm.
  46. // Target velocity.
  47. #define MOTOR_TARGET_VELOCITY_FORWARD 15000 // forwaed, 1500 rpm, uint: 0.1 rpm, maximum 50000.
  48. #define MOTOR_TARGET_VELOCITY_REVERSE -15000 // reverse, 1500 rpm, uint: 0.1 rpm, maximum 50000.
  49. // Slave num.
  50. #define SLAVE_NUM 1
  51. // PDO information of device, get from `sudo ethercat cstruct` in cmd line
  52. /* Master 0, Slave 0
  53. * Vendor ID: 0x000001dd
  54. * Product code: 0x10305070
  55. * Revision number: 0x02040608
  56. */
  57. // Slave alias, position, vendor ID and product ID.
  58. #define SLAVE_0_ALIAS 0
  59. #define SLAVE_0_POSITION 0
  60. #define SLAVE_0_VID_PID 0x000001dd, 0x10305070
  61. // Offsets for PDO entries.
  62. static struct _SlaveOffset
  63. {
  64. unsigned int Operation_Mode;
  65. unsigned int Ctrl_Word;
  66. unsigned int Target_Velocity;
  67. unsigned int Profile_Acceleration;
  68. unsigned int Profile_Deceleration;
  69. unsigned int Status_Word;
  70. unsigned int Current_Velocity;
  71. } slave_offset[SLAVE_NUM];
  72. struct _SlaveInfo
  73. {
  74. uint32_t Alias;
  75. uint32_t Position;
  76. uint32_t Vendor_ID;
  77. uint32_t Product_Code;
  78. };
  79. struct _SlaveInfo slave_info[] = {{SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID}};
  80. struct _SlaveConfig
  81. {
  82. // Slave configuration.
  83. ec_slave_config_t *sc;
  84. ec_slave_config_state_t sc_state;
  85. struct _SlaveOffset offset;
  86. int current_velocity;
  87. };
  88. struct _Domain
  89. {
  90. ec_domain_t *domain;
  91. ec_domain_state_t domain_state;
  92. // Domain process data.
  93. uint8_t *domain_pd;
  94. };
  95. // Configure the PDOs entries register
  96. const static ec_pdo_entry_reg_t domain_regs[] = {
  97. // Slave 0.
  98. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6040, 0, &slave_offset[0].Ctrl_Word},
  99. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6060, 0, &slave_offset[0].Operation_Mode},
  100. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x60FF, 0, &slave_offset[0].Target_Velocity},
  101. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6083, 0, &slave_offset[0].Profile_Acceleration},
  102. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6084, 0, &slave_offset[0].Profile_Deceleration},
  103. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x6041, 0, &slave_offset[0].Status_Word},
  104. {SLAVE_0_ALIAS, SLAVE_0_POSITION, SLAVE_0_VID_PID, 0x606B, 0, &slave_offset[0].Current_Velocity},
  105. {}};
  106. ec_pdo_entry_info_t slave_pdo_entries[] = {
  107. {0x6040, 0x00, 16}, // Control word.
  108. {0x6060, 0x00, 8}, // Operation mode.
  109. {0x60FF, 0x00, 32}, // Target velocity.
  110. {0x6083, 0x00, 32}, // Profile acceleration.
  111. {0x6084, 0x00, 32}, // Profile deceleeration.
  112. {0x6041, 0x00, 16}, // Status word.
  113. {0x606B, 0x00, 32}, // Current velocity.
  114. };
  115. ec_pdo_info_t slave_pdos[] = {
  116. {0x1600, 5, slave_pdo_entries + 0},
  117. {0x1a00, 2, slave_pdo_entries + 5},
  118. };
  119. ec_sync_info_t slave_syncs[] = {
  120. {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
  121. {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
  122. {2, EC_DIR_OUTPUT, 1, slave_pdos + 0, EC_WD_DISABLE},
  123. {3, EC_DIR_INPUT, 1, slave_pdos + 1, EC_WD_DISABLE},
  124. {0xff}};
  125. // Define EtherCAT master and corresponding states.
  126. static ec_master_t *master = NULL;
  127. static ec_master_state_t master_state = {};
  128. // Define process data of domain
  129. struct _Domain domain1;
  130. // Check process data state.
  131. void check_domain_state(struct _Domain *domain)
  132. {
  133. ec_domain_state_t ds;
  134. ecrt_domain_state(domain->domain, &ds);
  135. if (ds.working_counter != domain->domain_state.working_counter)
  136. {
  137. /* Periodically check the read/write status of the slave memory, and return 'working_counter + i' if it can be read and write.
  138. * send read-only command(xRD), if slave memory readable, 'working_counter + 1';
  139. * send write-only command(xWR), if slave memory writable, 'working_counter + 1';
  140. * send read-write command(xRW), if slave memory readable, 'working_counter + 1', if slave memory writable, 'working_counter + 2', that means 'working_counter + 3' ;
  141. */
  142. printf("Domain: WC %u.\n", ds.working_counter);
  143. }
  144. if (ds.wc_state != domain->domain_state.wc_state)
  145. {
  146. /*
  147. * 0: No registered process data were exchanged;
  148. * 1: Some of the registered process data were exchanged.
  149. * 2: All registered process data were exchanged.
  150. */
  151. printf("Domain: State %u.\n", ds.wc_state);
  152. }
  153. domain->domain_state = ds;
  154. }
  155. // Check for master state.
  156. void check_master_state()
  157. {
  158. ec_master_state_t ms;
  159. ecrt_master_state(master, &ms);
  160. if (ms.slaves_responding != master_state.slaves_responding)
  161. {
  162. // Sum of responding slaves on all Ethernet devices.
  163. printf("%u slave(s).\n", ms.slaves_responding);
  164. }
  165. if (ms.al_states != master_state.al_states)
  166. {
  167. // Application-layer states of all slaves. 1:init; 2:preop; 4:safeop; 8:op;
  168. printf("AL states: 0x%02X.\n", ms.al_states);
  169. }
  170. if (ms.link_up != master_state.link_up)
  171. {
  172. printf("Link is %s.\n", ms.link_up ? "up" : "down");
  173. }
  174. master_state = ms;
  175. }
  176. // Check for slave configuration state.
  177. void check_slave_config_state(struct _SlaveConfig *slave_config)
  178. {
  179. ec_slave_config_state_t s;
  180. int i;
  181. for (i = 0; i < SLAVE_NUM; i++)
  182. {
  183. memset(&s, 0, sizeof(s));
  184. ecrt_slave_config_state(slave_config[i].sc, &s);
  185. if (s.al_state != slave_config[i].sc_state.al_state)
  186. {
  187. // The application-layer state of the slave. 1:init; 2:preop; 4:safeop; 8:op;
  188. printf("slave[%d]: State 0x%02X.\n", i, s.al_state);
  189. }
  190. if (s.online != slave_config[i].sc_state.online)
  191. {
  192. printf("slave[%d]: %s.\n", i, s.online ? "online" : "offline");
  193. }
  194. if (s.operational != slave_config[i].sc_state.operational)
  195. {
  196. // The slave was brought into \a OP state using the specified configuration.
  197. printf("slave[%d]: %soperational.\n", i, s.operational ? "" : "Not ");
  198. }
  199. slave_config[i].sc_state = s;
  200. }
  201. }
  202. struct timespec timespec_add(struct timespec time1, struct timespec time2)
  203. {
  204. struct timespec result;
  205. if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC)
  206. {
  207. result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
  208. result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
  209. }
  210. else
  211. {
  212. result.tv_sec = time1.tv_sec + time2.tv_sec;
  213. result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
  214. }
  215. return result;
  216. }
  217. // ************************************************************************
  218. void cyclic_task(struct _SlaveConfig *slave_config, struct _Domain *domain)
  219. {
  220. // Used to determine the value of the status word.
  221. uint16_t proof_value = 0x004F;
  222. uint16_t ctrl_word[] = {0x0006, 0x0007, 0x000f};
  223. uint16_t status;
  224. uint32_t count = 0;
  225. static int direction = 0;
  226. struct timespec wakeupTime;
  227. #ifdef MEASURE_TIMING
  228. struct timespec startTime, endTime, lastStartTime = {};
  229. uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0, latency_min_ns = 0, latency_max_ns = 0,
  230. period_min_ns = 0, period_max_ns = 0, exec_min_ns = 0, exec_max_ns = 0, period_max_print = 0;
  231. #endif
  232. int i;
  233. // Get current time.
  234. clock_gettime(CLOCK_TO_USE, &wakeupTime);
  235. while (!is_quit)
  236. {
  237. // Period time: 1ms.
  238. wakeupTime = timespec_add(wakeupTime, cycletime);
  239. clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
  240. #ifdef MEASURE_TIMING
  241. clock_gettime(CLOCK_TO_USE, &startTime);
  242. // The time interval of receiving and sending EtherCAT data.
  243. latency_ns = DIFF_NS(wakeupTime, startTime);
  244. // EtherCAT communication cycle.
  245. period_ns = DIFF_NS(lastStartTime, startTime);
  246. // Wait time for wake up.
  247. exec_ns = DIFF_NS(lastStartTime, endTime);
  248. lastStartTime = startTime;
  249. #endif
  250. ecrt_master_receive(master);
  251. ecrt_domain_process(domain->domain);
  252. check_domain_state(domain);
  253. check_master_state();
  254. check_slave_config_state(slave_config);
  255. #ifdef MEASURE_TIMING
  256. latency_max_ns = (latency_ns > latency_max_ns) ? latency_ns : latency_max_ns;
  257. latency_min_ns = (latency_ns < latency_min_ns) ? latency_ns : latency_min_ns;
  258. period_max_ns = (period_ns > period_max_ns) ? period_ns : period_max_ns;
  259. period_min_ns = (period_ns < period_min_ns) ? period_ns : period_min_ns;
  260. exec_max_ns = (exec_ns > exec_max_ns) ? exec_ns : exec_max_ns;
  261. exec_min_ns = (exec_ns < exec_min_ns) ? exec_ns : exec_min_ns;
  262. // Do not print frequently, or it will affect real-time performance.
  263. count++;
  264. if (count == FREQUENCY)
  265. {
  266. // Count the number of times the period exceeds 1100000ns and avoid the impact within the first second.
  267. if (log_item_id > 1 && period_max_ns >= 1100000)
  268. num_of_timeouts++;
  269. // output timing stats
  270. printf("period\tmin / max = %u / %u(ns)\n", period_min_ns, period_max_ns);
  271. printf("exec\tmin / max = %u / %u(ns)\n", exec_min_ns, exec_max_ns);
  272. printf("latency\tmin / max = %u / %u(ns)\n\n", latency_min_ns, latency_max_ns);
  273. if ((log_item_id++) > 1)
  274. {
  275. period_max_print = (period_max_print > period_max_ns) ? period_max_print : period_max_ns;
  276. printf("number of timeouts=%d, maximum period=%u\n\n", num_of_timeouts, period_max_print);
  277. }
  278. fprintf(logfile, "%d,%u,%u,%u,%u,%u,%u\r\n", log_item_id, period_min_ns, period_max_ns, exec_min_ns, exec_max_ns, latency_min_ns, latency_max_ns);
  279. period_max_ns = 0;
  280. period_min_ns = 0xffffffff;
  281. exec_max_ns = 0;
  282. exec_min_ns = 0xffffffff;
  283. latency_max_ns = 0;
  284. latency_min_ns = 0xffffffff;
  285. count = 0;
  286. }
  287. #endif
  288. for (i = SLAVE_NUM - 1; i >= 0; i--)
  289. {
  290. // Read state word.
  291. status = EC_READ_U16(domain->domain_pd + slave_offset[i].Status_Word);
  292. if ((status & proof_value) == 0x0040)
  293. {
  294. // Set operation mode to velocity Mode.
  295. EC_WRITE_U8(domain->domain_pd + slave_offset[i].Operation_Mode, MOTOR_MODEL_PROFILE_VELOCITY);
  296. // Set Profile_Acceleration and Profile_Deceleration.
  297. EC_WRITE_U32(domain->domain_pd + slave_offset[i].Profile_Acceleration, MOTOR_PROFILE_ACCELERATION);
  298. EC_WRITE_U32(domain->domain_pd + slave_offset[i].Profile_Deceleration, MOTOR_PROFILE_DECELERATION);
  299. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[0]);
  300. proof_value = 0x006F;
  301. }
  302. else if ((status & proof_value) == 0x0021)
  303. {
  304. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[1]);
  305. proof_value = 0x006F;
  306. }
  307. else if ((status & proof_value) == 0x0023)
  308. {
  309. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, ctrl_word[2]);
  310. proof_value = 0x006F;
  311. }
  312. else if ((status & proof_value) == 0x0027)
  313. {
  314. slave_config[i].current_velocity = EC_READ_U32(domain->domain_pd + slave_offset[i].Current_Velocity);
  315. // Determine whether to turn forward or reverse.
  316. if (direction == 0)
  317. {
  318. // forward.
  319. if (slave_config[i].current_velocity == 0)
  320. {
  321. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, MOTOR_TARGET_VELOCITY_FORWARD);
  322. }
  323. else if (slave_config[i].current_velocity == MOTOR_TARGET_VELOCITY_FORWARD)
  324. {
  325. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, 0);
  326. direction = 1;
  327. }
  328. }
  329. else if (direction == 1)
  330. {
  331. // reverse.
  332. if (slave_config[i].current_velocity == 0)
  333. {
  334. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, MOTOR_TARGET_VELOCITY_REVERSE);
  335. }
  336. else if (slave_config[i].current_velocity == MOTOR_TARGET_VELOCITY_REVERSE)
  337. {
  338. EC_WRITE_S32(domain->domain_pd + slave_offset[i].Target_Velocity, 0);
  339. direction = 0;
  340. }
  341. }
  342. }
  343. else
  344. {
  345. // Fault reset.
  346. EC_WRITE_U16(domain->domain_pd + slave_offset[i].Ctrl_Word, 0x008f);
  347. }
  348. }
  349. // Send process data.
  350. ecrt_domain_queue(domain->domain);
  351. ecrt_master_send(master);
  352. #ifdef MEASURE_TIMING
  353. clock_gettime(CLOCK_TO_USE, &endTime);
  354. #endif
  355. }
  356. }
  357. // Function stop servo.
  358. void stop_servo()
  359. {
  360. int i;
  361. // Receive process data.
  362. ecrt_master_receive(master);
  363. ecrt_domain_process(domain1.domain);
  364. for (i = 0; i < SLAVE_NUM; i++)
  365. {
  366. EC_WRITE_U16(domain1.domain_pd + slave_offset[i].Ctrl_Word, MOTOR_MODEL_CONTROL_WORD_HALT);
  367. }
  368. // Send process data, queues all domain datagrams in the master's datagram queue.
  369. ecrt_domain_queue(domain1.domain);
  370. // Sends all datagrams in the queue.
  371. ecrt_master_send(master);
  372. }
  373. // Function exit program.
  374. static void sig_handle(int signal)
  375. {
  376. is_quit = true;
  377. stop_servo();
  378. fclose(logfile);
  379. printf("The log file is saved as %s\n", logname);
  380. printf("Manually exit the program!\n");
  381. }
  382. // Function error leaving program.
  383. void err_leave(void)
  384. {
  385. // Releases EtherCAT master.
  386. ecrt_release_master(master);
  387. printf("Release master.\n");
  388. fclose(logfile);
  389. exit(EXIT_FAILURE);
  390. }
  391. // Function initial the ethercat.
  392. int init_ethercat(struct _SlaveConfig *slave_config, int *ret, int *status)
  393. {
  394. int i;
  395. // Request EtherCAT master.
  396. master = ecrt_request_master(0);
  397. if (!master)
  398. {
  399. printf("Request master failed.\n");
  400. return -1;
  401. }
  402. printf("Request master success.\n");
  403. memset(&domain1, 0, sizeof(domain1));
  404. // Create domain.
  405. domain1.domain = ecrt_master_create_domain(master);
  406. if (!domain1.domain)
  407. {
  408. *status = -1;
  409. printf("Create domain failed.\n");
  410. err_leave();
  411. }
  412. printf("Create domain success.\n");
  413. // Get slave configuration.
  414. for (i = 0; i < SLAVE_NUM; i++)
  415. {
  416. slave_config[i].sc = ecrt_master_slave_config(master, slave_info[i].Alias,
  417. slave_info[i].Position, slave_info[i].Vendor_ID,
  418. slave_info[i].Product_Code);
  419. if (!slave_config[i].sc)
  420. {
  421. *status = -1;
  422. printf("Get slave configuration failed.\n");
  423. err_leave();
  424. }
  425. }
  426. printf("Get slave configuration success.\n");
  427. // Configuring PDO.
  428. for (i = 0; i < SLAVE_NUM; i++)
  429. {
  430. *ret = ecrt_slave_config_pdos(slave_config[i].sc, EC_END, slave_syncs);
  431. if (*ret != 0)
  432. {
  433. *status = -1;
  434. printf("Configuration PDO failed.\n");
  435. err_leave();
  436. }
  437. }
  438. printf("Configuration PDO success.\n");
  439. // Registers a bunch of PDO entries for a domain.
  440. *ret = ecrt_domain_reg_pdo_entry_list(domain1.domain, domain_regs);
  441. if (*ret != 0)
  442. {
  443. *status = -1;
  444. printf("Failed to register bunch of PDO entries for domain.\n");
  445. err_leave();
  446. }
  447. printf("Success to register bunch of PDO entries for domain.\n");
  448. // Activate EtherCAT master.
  449. *ret = ecrt_master_activate(master);
  450. if (*ret < 0)
  451. {
  452. *status = -1;
  453. printf("Activate master failed.\n");
  454. err_leave();
  455. }
  456. printf("Activate master success.\n");
  457. // Get Pointer to the process data memory.
  458. domain1.domain_pd = ecrt_domain_data(domain1.domain);
  459. if (!domain1.domain_pd)
  460. {
  461. *status = -1;
  462. printf("Get pointer to the process data memory failed.\n");
  463. err_leave();
  464. }
  465. printf("Get pointer to the process data memory success.\n");
  466. return 0;
  467. }
  468. int main(int argc, char **argv)
  469. {
  470. int status = 0, ret = -1;
  471. struct _SlaveConfig slave_config[SLAVE_NUM];
  472. if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
  473. {
  474. perror("mlockall failed");
  475. return -1;
  476. }
  477. // Ctrl+c handler.
  478. signal(SIGINT, sig_handle);
  479. memset(&slave_config, 0, sizeof(slave_config));
  480. // Create a .csv file named by time.
  481. time_t current_time;
  482. struct tm *time_info;
  483. char time_buffer[20];
  484. time(&current_time);
  485. time_info = localtime(&current_time);
  486. strftime(time_buffer, sizeof(time_buffer), "%Y%m%d%H%M%S", time_info);
  487. snprintf(logname, sizeof(logname), "%s.csv", time_buffer);
  488. logfile= fopen(logname, "w");
  489. if (logfile == NULL)
  490. {
  491. perror("Error opening file");
  492. return 1;
  493. }
  494. // Defining List headers.
  495. fwrite("ID, period_min,period_max,exec_min,exec_max,latency_min,latency_max,\r\n", 1, strlen("ID, period_min,period_max,exec_min,exec_max,latency_min,latency_max,\r\n"), logfile);
  496. // init ethercat
  497. init_ethercat(slave_config, &ret, &status);
  498. // Set highest priority.
  499. pid_t pid = getpid();
  500. struct sched_param param;
  501. int max_priority = sched_get_priority_max(SCHED_FIFO);
  502. int min_priority = sched_get_priority_min(SCHED_FIFO);
  503. param.sched_priority = max_priority;
  504. if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1) {
  505. perror("sched_setscheduler");
  506. exit(EXIT_FAILURE);
  507. }
  508. printf("Max priority: %d\n", max_priority);
  509. printf("Min priority: %d\n", min_priority);
  510. printf("Current priority: %d\n", sched_get_priority_max(SCHED_FIFO));
  511. // Start cyclic function.
  512. printf("Run cycle task...\n");
  513. cyclic_task(slave_config, &domain1);
  514. return status;
  515. }