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