etherlabmaster: examples/rtai_rtdm_dc/main.c@bb2163e6b145 (annotated)

2447 e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	1	/******************************************************************************
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	2	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	3	* $Id$
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	4	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	5	* Copyright (C) 2011 IgH Andreas Stewering-Bone
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	6	* 2012 Florian Pose <fp@igh-essen.com>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	7	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	8	* This file is part of the IgH EtherCAT master
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	9	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	10	* The IgH EtherCAT Master is free software; you can redistribute it and/or
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	11	* modify it under the terms of the GNU General Public License version 2, as
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	12	* published by the Free Software Foundation.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	13	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	14	* The IgH EtherCAT master is distributed in the hope that it will be useful,
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	15	* but WITHOUT ANY WARRANTY; without even the implied warranty of
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	16	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	17	* Public License for more details.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	18	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	19	* You should have received a copy of the GNU General Public License along
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	20	* with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	21	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	22	* ---
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	23	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	24	* The license mentioned above concerns the source code only. Using the
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	25	* EtherCAT technology and brand is only permitted in compliance with the
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	26	* industrial property and similar rights of Beckhoff Automation GmbH.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	27	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	28	*****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	29
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	30	#include <sched.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	31	#include <stdio.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	32	#include <stdlib.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	33	#include <fcntl.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	34	#include <signal.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	35
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	36	#include <rtai_lxrt.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	37	#include <rtdm/rtdm.h>
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	38
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	39	#include "ecrt.h"
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	40
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	41	#define rt_printf(X, Y)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	42
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	43	#define NSEC_PER_SEC 1000000000
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	44
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	45	RT_TASK *task;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	46
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	47	static unsigned int cycle_ns = 1000000; /* 1 ms */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	48
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	49	static int run = 1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	50
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	51	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	52
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	53	// EtherCAT
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	54	static ec_master_t *master = NULL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	55	static ec_master_state_t master_state = {};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	56
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	57	static ec_domain_t *domain1 = NULL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	58	static ec_domain_state_t domain1_state = {};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	59
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	60	static uint8_t *domain1_pd = NULL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	61
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	62	static ec_slave_config_t *sc_dig_out_01 = NULL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	63
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	64	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	65
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	66	// EtherCAT distributed clock variables
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	67
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	68	#define DC_FILTER_CNT 1024
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	69	#define SYNC_MASTER_TO_REF 1
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	70
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	71	static uint64_t dc_start_time_ns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	72	static uint64_t dc_time_ns = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	73	#if SYNC_MASTER_TO_REF
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	74	static uint8_t dc_started = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	75	static int32_t dc_diff_ns = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	76	static int32_t prev_dc_diff_ns = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	77	static int64_t dc_diff_total_ns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	78	static int64_t dc_delta_total_ns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	79	static int dc_filter_idx = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	80	static int64_t dc_adjust_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	81	#endif
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	82	static int64_t system_time_base = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	83	static uint64_t wakeup_time = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	84	static uint64_t overruns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	85
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	86	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	87
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	88	// process data
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	89
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	90	#define BusCoupler01_Pos 0, 0
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	91	#define DigOutSlave01_Pos 0, 1
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	92
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	93	#define Beckhoff_EK1100 0x00000002, 0x044c2c52
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	94	#define Beckhoff_EL2004 0x00000002, 0x07d43052
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	95
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	96	// offsets for PDO entries
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	97	static unsigned int off_dig_out0 = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	98
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	99	// process data
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	100
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	101	const static ec_pdo_entry_reg_t domain1_regs[] = {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	102	{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL},
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	103	{}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	104	};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	105
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	106	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	107
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	108	/* Slave 1, "EL2004"
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	109	* Vendor ID: 0x00000002
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	110	* Product code: 0x07d43052
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	111	* Revision number: 0x00100000
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	112	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	113
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	114	ec_pdo_entry_info_t slave_1_pdo_entries[] = {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	115	{0x7000, 0x01, 1}, /* Output */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	116	{0x7010, 0x01, 1}, /* Output */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	117	{0x7020, 0x01, 1}, /* Output */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	118	{0x7030, 0x01, 1}, /* Output */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	119	};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	120
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	121	ec_pdo_info_t slave_1_pdos[] = {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	122	{0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	123	{0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	124	{0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	125	{0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	126	};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	127
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	128	ec_sync_info_t slave_1_syncs[] = {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	129	{0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE},
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	130	{0xff}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	131	};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	132
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	133	/*****************************************************************************
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	134	* Realtime task
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	135	****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	136
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	137	/** Get the time in ns for the current cpu, adjusted by system_time_base.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	138	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	139	* \attention Rather than calling rt_get_time_ns() directly, all application
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	140	* time calls should use this method instead.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	141	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	142	* \ret The time in ns.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	143	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	144	uint64_t system_time_ns(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	145	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	146	RTIME time = rt_get_time_ns();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	147
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	148	if (system_time_base > time) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	149	rt_printk("%s() error: system_time_base greater than"
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	150	" system time (system_time_base: %lld, time: %llu\n",
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	151	__func__, system_time_base, time);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	152	return time;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	153	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	154	else {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	155	return time - system_time_base;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	156	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	157	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	158
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	159	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	160
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	161	/** Convert system time to RTAI time in counts (via the system_time_base).
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	162	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	163	RTIME system2count(
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	164	uint64_t time
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	165	)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	166	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	167	RTIME ret;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	168
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	169	if ((system_time_base < 0) &&
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	170	((uint64_t) (-system_time_base) > time)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	171	rt_printk("%s() error: system_time_base less than"
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	172	" system time (system_time_base: %lld, time: %llu\n",
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	173	__func__, system_time_base, time);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	174	ret = time;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	175	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	176	else {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	177	ret = time + system_time_base;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	178	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	179
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	180	return nano2count(ret);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	181	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	182
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	183	/*****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	184
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	185	/** Synchronise the distributed clocks
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	186	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	187	void sync_distributed_clocks(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	188	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	189	#if SYNC_MASTER_TO_REF
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	190	uint32_t ref_time = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	191	uint64_t prev_app_time = dc_time_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	192	#endif
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	193
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	194	dc_time_ns = system_time_ns();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	195
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	196	// set master time in nano-seconds
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	197	ecrt_master_application_time(master, dc_time_ns);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	198
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	199	#if SYNC_MASTER_TO_REF
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	200	// get reference clock time to synchronize master cycle
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	201	ecrt_master_reference_clock_time(master, &ref_time);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	202	dc_diff_ns = (uint32_t) prev_app_time - ref_time;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	203	#else
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	204	// sync reference clock to master
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	205	ecrt_master_sync_reference_clock(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	206	#endif
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	207
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	208	// call to sync slaves to ref slave
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	209	ecrt_master_sync_slave_clocks(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	210	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	211
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	212	/*****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	213
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	214	/** Return the sign of a number
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	215	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	216	* ie -1 for -ve value, 0 for 0, +1 for +ve value
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	217	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	218	* \retval the sign of the value
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	219	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	220	#define sign(val) \
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	221	({ typeof (val) _val = (val); \
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	222	((_val > 0) - (_val < 0)); })
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	223
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	224	/*****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	225
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	226	/** Update the master time based on ref slaves time diff
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	227	*
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	228	* called after the ethercat frame is sent to avoid time jitter in
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	229	* sync_distributed_clocks()
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	230	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	231	void update_master_clock(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	232	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	233	#if SYNC_MASTER_TO_REF
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	234	// calc drift (via un-normalised time diff)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	235	int32_t delta = dc_diff_ns - prev_dc_diff_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	236	prev_dc_diff_ns = dc_diff_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	237
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	238	// normalise the time diff
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	239	dc_diff_ns =
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	240	((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	241
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	242	// only update if primary master
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	243	if (dc_started) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	244
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	245	// add to totals
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	246	dc_diff_total_ns += dc_diff_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	247	dc_delta_total_ns += delta;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	248	dc_filter_idx++;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	249
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	250	if (dc_filter_idx >= DC_FILTER_CNT) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	251	// add rounded delta average
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	252	dc_adjust_ns +=
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	253	((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	254
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	255	// and add adjustment for general diff (to pull in drift)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	256	dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	257
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	258	// limit crazy numbers (0.1% of std cycle time)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	259	if (dc_adjust_ns < -1000) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	260	dc_adjust_ns = -1000;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	261	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	262	if (dc_adjust_ns > 1000) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	263	dc_adjust_ns = 1000;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	264	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	265
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	266	// reset
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	267	dc_diff_total_ns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	268	dc_delta_total_ns = 0LL;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	269	dc_filter_idx = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	270	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	271
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	272	// add cycles adjustment to time base (including a spot adjustment)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	273	system_time_base += dc_adjust_ns + sign(dc_diff_ns);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	274	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	275	else {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	276	dc_started = (dc_diff_ns != 0);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	277
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	278	if (dc_started) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	279	// output first diff
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	280	rt_printk("First master diff: %d.\n", dc_diff_ns);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	281
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	282	// record the time of this initial cycle
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	283	dc_start_time_ns = dc_time_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	284	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	285	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	286	#endif
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	287	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	288
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	289	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	290
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	291	void rt_check_domain_state(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	292	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	293	ec_domain_state_t ds = {};
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	294
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	295	ecrt_domain_state(domain1, &ds);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	296
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	297	if (ds.working_counter != domain1_state.working_counter) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	298	rt_printf("Domain1: WC %u.\n", ds.working_counter);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	299	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	300
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	301	if (ds.wc_state != domain1_state.wc_state) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	302	rt_printf("Domain1: State %u.\n", ds.wc_state);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	303	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	304
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	305	domain1_state = ds;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	306	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	307
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	308	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	309
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	310	void rt_check_master_state(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	311	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	312	ec_master_state_t ms;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	313
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	314	ecrt_master_state(master, &ms);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	315
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	316	if (ms.slaves_responding != master_state.slaves_responding) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	317	rt_printf("%u slave(s).\n", ms.slaves_responding);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	318	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	319
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	320	if (ms.al_states != master_state.al_states) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	321	rt_printf("AL states: 0x%02X.\n", ms.al_states);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	322	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	323
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	324	if (ms.link_up != master_state.link_up) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	325	rt_printf("Link is %s.\n", ms.link_up ? "up" : "down");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	326	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	327
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	328	master_state = ms;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	329	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	330
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	331	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	332
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	333	/** Wait for the next period
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	334	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	335	void wait_period(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	336	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	337	while (1)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	338	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	339	RTIME wakeup_count = system2count(wakeup_time);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	340	RTIME current_count = rt_get_time();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	341
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	342	if ((wakeup_count < current_count)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	343	\|\| (wakeup_count > current_count + (50 * cycle_ns))) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	344	rt_printk("%s(): unexpected wake time!\n", __func__);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	345	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	346
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	347	switch (rt_sleep_until(wakeup_count)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	348	case RTE_UNBLKD:
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	349	rt_printk("rt_sleep_until(): RTE_UNBLKD\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	350	continue;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	351
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	352	case RTE_TMROVRN:
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	353	rt_printk("rt_sleep_until(): RTE_TMROVRN\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	354	overruns++;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	355
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	356	if (overruns % 100 == 0) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	357	// in case wake time is broken ensure other processes get
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	358	// some time slice (and error messages can get displayed)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	359	rt_sleep(cycle_ns / 100);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	360	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	361	break;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	362
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	363	default:
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	364	break;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	365	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	366
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	367	// done if we got to here
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	368	break;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	369	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	370
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	371	// calc next wake time (in sys time)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	372	wakeup_time += cycle_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	373	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	374
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	375	/****************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	376
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	377	void my_cyclic(void)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	378	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	379	int cycle_counter = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	380	unsigned int blink = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	381
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	382	// oneshot mode to allow adjustable wake time
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	383	rt_set_oneshot_mode();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	384
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	385	// set first wake time in a few cycles
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	386	wakeup_time = system_time_ns() + 10 * cycle_ns;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	387
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	388	// start the timer
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	389	start_rt_timer(nano2count(cycle_ns));
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	390
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	391	rt_make_hard_real_time();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	392
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	393	while (run) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	394	// wait for next period (using adjustable system time)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	395	wait_period();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	396
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	397	cycle_counter++;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	398
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	399	if (!run) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	400	break;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	401	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	402
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	403	// receive EtherCAT
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	404	ecrt_master_receive(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	405	ecrt_domain_process(domain1);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	406
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	407	rt_check_domain_state();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	408
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	409	if (!(cycle_counter % 1000)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	410	rt_check_master_state();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	411	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	412
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	413	if (!(cycle_counter % 200)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	414	blink = !blink;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	415	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	416
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	417	EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	418
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	419	// queue process data
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	420	ecrt_domain_queue(domain1);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	421
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	422	// sync distributed clock just before master_send to set
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	423	// most accurate master clock time
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	424	sync_distributed_clocks();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	425
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	426	// send EtherCAT data
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	427	ecrt_master_send(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	428
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	429	// update the master clock
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	430	// Note: called after ecrt_master_send() to reduce time
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	431	// jitter in the sync_distributed_clocks() call
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	432	update_master_clock();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	433	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	434
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	435	rt_make_soft_real_time();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	436	stop_rt_timer();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	437	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	438
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	439	/****************************************************************************
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	440	* Signal handler
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	441	***************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	442
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	443	void signal_handler(int sig)
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	444	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	445	run = 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	446	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	447
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	448	/****************************************************************************
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	449	* Main function
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	450	***************************************************************************/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	451
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	452	int main(int argc, char *argv[])
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	453	{
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	454	ec_slave_config_t *sc_ek1100;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	455	int ret;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	456
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	457	signal(SIGTERM, signal_handler);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	458	signal(SIGINT, signal_handler);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	459
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	460	mlockall(MCL_CURRENT \| MCL_FUTURE);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	461
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	462	printf("Requesting master...\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	463	master = ecrt_request_master(0);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	464	if (!master) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	465	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	466	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	467
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	468	domain1 = ecrt_master_create_domain(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	469	if (!domain1) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	470	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	471	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	472
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	473	printf("Creating slave configurations...\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	474
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	475	// Create configuration for bus coupler
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	476	sc_ek1100 =
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	477	ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	478	if (!sc_ek1100) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	479	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	480	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	481
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	482	sc_dig_out_01 =
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	483	ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	484	if (!sc_dig_out_01) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	485	fprintf(stderr, "Failed to get slave configuration.\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	486	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	487	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	488
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	489	if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	490	fprintf(stderr, "Failed to configure PDOs.\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	491	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	492	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	493
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	494	if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	495	fprintf(stderr, "PDO entry registration failed!\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	496	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	497	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	498
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	499	/* Set the initial master time and select a slave to use as the DC
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	500	* reference clock, otherwise pass NULL to auto select the first capable
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	501	* slave. Note: This can be used whether the master or the ref slave will
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	502	* be used as the systems master DC clock.
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	503	*/
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	504	dc_start_time_ns = system_time_ns();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	505	dc_time_ns = dc_start_time_ns;
2523 c5c81a52fc30 Improved API documentation. Florian Pose <fp@igh-essen.com> parents: 2447 diff changeset	506
c5c81a52fc30 Improved API documentation. Florian Pose <fp@igh-essen.com> parents: 2447 diff changeset	507	/* Attention: The initial application time is also used for phase
c5c81a52fc30 Improved API documentation. Florian Pose <fp@igh-essen.com> parents: 2447 diff changeset	508	* calculation for the SYNC0/1 interrupts. Please be sure to call it at
c5c81a52fc30 Improved API documentation. Florian Pose <fp@igh-essen.com> parents: 2447 diff changeset	509	* the correct phase to the realtime cycle.
c5c81a52fc30 Improved API documentation. Florian Pose <fp@igh-essen.com> parents: 2447 diff changeset	510	*/
2447 e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	511	ecrt_master_application_time(master, dc_start_time_ns);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	512
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	513	ret = ecrt_master_select_reference_clock(master, sc_ek1100);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	514	if (ret < 0) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	515	fprintf(stderr, "Failed to select reference clock: %s\n",
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	516	strerror(-ret));
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	517	return ret;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	518	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	519
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	520	printf("Activating master...\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	521	if (ecrt_master_activate(master)) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	522	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	523	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	524
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	525	if (!(domain1_pd = ecrt_domain_data(domain1))) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	526	fprintf(stderr, "Failed to get domain data pointer.\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	527	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	528	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	529
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	530	/* Create cyclic RT-thread */
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	531	struct sched_param param;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	532	param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	533	if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	534	puts("ERROR IN SETTING THE SCHEDULER");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	535	perror("errno");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	536	return -1;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	537	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	538
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	539	task = rt_task_init(nam2num("ec_rtai_rtdm_example"),
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	540	0 /* priority /, 0 / stack size /, 0 / msg size */);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	541
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	542	my_cyclic();
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	543
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	544	rt_task_delete(task);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	545
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	546	printf("End of Program\n");
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	547	ecrt_release_master(master);
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	548
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	549	return 0;
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	550	}
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	551
e93efb4af231 Added interface to select the reference clock and to sync to it. Florian Pose <fp@igh-essen.com> parents: diff changeset	552	/****************************************************************************/

author	Florian Pose <fp@igh-essen.com>
	Fri, 16 Sep 2016 11:57:01 +0200
branch	stable-1.5
changeset 2657	bb2163e6b145
parent 2523	c5c81a52fc30
permissions	-rw-r--r--