RTUtility.cpp
Go to the documentation of this file.
1 #include "RTUtility.h"
2 
3 #include <errno.h>
4 
5 #include <thread>
6 
7 #include <fcntl.h>
8 #include <sched.h>
9 #include <sys/stat.h>
10 #include <sys/syscall.h>
11 
13 
15 {
16  bool
18  {
19  long pid = syscall(SYS_gettid);
20  ARMARX_INFO << "Elevating priority of thread #" << pid << " to " << priority;
21  ARMARX_INFO << "Priority before: " << sched_getscheduler(static_cast<int>(pid));
22  struct sched_param param;
23  param.sched_priority = priority;
24  if (sched_setscheduler(static_cast<int>(pid), SCHED_FIFO | SCHED_RESET_ON_FORK, &param) ==
25  -1)
26  {
27  int error = errno;
28  ARMARX_WARNING << "sched_setscheduler failed: " << std::string(strerror(error));
29  return false;
30  }
31  if (sched_getparam(static_cast<int>(pid), &param) == -1)
32  {
33  int error = errno;
34  ARMARX_WARNING << "sched_getparam failed: " << std::string(strerror(error));
35  return false;
36  }
37  int new_priority = param.sched_priority;
38  if (new_priority == priority)
39  {
40  ARMARX_IMPORTANT << "Successfully elevated priority of thread #" << pid
41  << " to new priority " << new_priority;
42  }
43  else
44  {
45  ARMARX_ERROR << "Failed to elevate priority of thread #" << pid;
46  }
47 
48  return true;
49  }
50 
51  bool
52  RTUtility::pinThreadToCPU(unsigned int cpu)
53  {
54  long pid = syscall(SYS_gettid);
55  unsigned int availableCores = std::thread::hardware_concurrency();
56  if (availableCores < cpu)
57  {
58  ARMARX_ERROR << "Trying to pin thread #" << pid << " to CPU #" << cpu << ", but only "
59  << availableCores << " cores are available";
60  return false;
61  }
62 
63  ARMARX_INFO << "Pinning thread #" << pid << " to CPU #" << cpu;
64  cpu_set_t mask;
65  CPU_ZERO(&mask);
66  CPU_SET(cpu, &mask);
67  int retval = sched_setaffinity(static_cast<int>(pid), sizeof(mask), &mask);
68  if (retval != 0)
69  {
70  ARMARX_ERROR << "Failed to pin thread #" << pid << " to CPU #" << cpu;
71  return false;
72  }
73  cpu_set_t mask2;
74  CPU_ZERO(&mask2);
75  CPU_SET(cpu, &mask2);
76  sched_getaffinity(static_cast<int>(pid), sizeof(mask2), &mask2);
77  bool matches = CPU_EQUAL(&mask, &mask2);
78  if (matches)
79  {
80  ARMARX_IMPORTANT << "Successfully pinned thread #" << pid << " to CPU #" << cpu;
81  return true;
82  }
83  else
84  {
85  ARMARX_ERROR << "Failed to pin thread #" << pid << " to CPU #" << cpu;
86  return false;
87  }
88  }
89 
90  static int fd_low_latency_target = -1;
91  static constexpr std::int32_t LOW_LATENCY_MAX_RESPONSE_TIME = 0;
92 
93 
94  bool
96  {
97  ARMARX_INFO << "Starting low latency mode by writing " << LOW_LATENCY_MAX_RESPONSE_TIME
98  << " to /dev/cpu_dma_latency and keeping the file open";
99 
100  struct stat s;
101  int err;
102  err = stat("/dev/cpu_dma_latency", &s);
103  if (err == -1)
104  {
105  ARMARX_WARNING << "stat /dev/cpu_dma_latency failed: " << strerror(errno);
106  return false;
107  }
108  fd_low_latency_target = open("/dev/cpu_dma_latency", O_RDWR);
109  if (fd_low_latency_target == -1)
110  {
111  ARMARX_WARNING << "open /dev/cpu_dma_latency failed: " << strerror(errno);
112  return false;
113  }
114 
115  err = static_cast<int>(write(fd_low_latency_target,
116  &LOW_LATENCY_MAX_RESPONSE_TIME,
117  sizeof(LOW_LATENCY_MAX_RESPONSE_TIME)));
118  if (err < 1)
119  {
120  ARMARX_WARNING << "error writing " << LOW_LATENCY_MAX_RESPONSE_TIME
121  << " to /dev/cpu_dma_latency";
122  close(fd_low_latency_target);
123  return false;
124  }
125  ARMARX_IMPORTANT << "Successfully set /dev/cpu_dma_latency to "
126  << LOW_LATENCY_MAX_RESPONSE_TIME << " µs\n";
127  return true;
128  }
129 
130  bool
132  {
133  ARMARX_INFO << "Stopping low latency mode by closing file /dev/cpu_dma_latency";
134  if (fd_low_latency_target >= 0)
135  {
136  close(fd_low_latency_target);
137  return true;
138  }
139  else
140  {
141  ARMARX_WARNING << "Low latency mode not active!";
142  return false;
143  }
144  }
145 
146 
147 } // namespace armarx::control::ethercat
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::control::ethercat::RTUtility::startLowLatencyMode
static bool startLowLatencyMode()
Activate low latency mode of the system.
Definition: RTUtility.cpp:95
armarx::control::ethercat::RTUtility::pinThreadToCPU
static bool pinThreadToCPU(unsigned int cpu)
Pins the calling thread to the CPU with the given id.
Definition: RTUtility.cpp:52
armarx::control::ethercat::RTUtility::stopLowLatencyMode
static bool stopLowLatencyMode()
Deactivate low latency mode of the system.
Definition: RTUtility.cpp:131
armarx::control::ethercat
Definition: Bus.cpp:24
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::ethercat::RTUtility::elevateThreadPriority
static bool elevateThreadPriority(int priority)
Elevate the thread priority of the calling thread to the given priority.
Definition: RTUtility.cpp:17
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::skills::gui::matches
bool matches(std::string skillName, std::vector< std::string > &searches)
Definition: SkillManagerWrapper.cpp:59
armarx::aron::write
requires data::isWriter< WriterT > void write(WriterT &aron_w, const Eigen::Matrix< EigenT, rows, cols, options > &input, typename WriterT::ReturnType &ret, const armarx::aron::Path &aron_p=armarx::aron::Path())
Definition: eigen.h:134
Logging.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
RTUtility.h