28#include <sys/resource.h>
42#include "HelperFunctionsCpp.h"
43#include "SyncInterrupt.h"
46constexpr int32_t SYNC_INTERRUPT_EXIT_ERROR = 5;
48constexpr int32_t SLOW_LOOP_MILLISECONDS = 50;
50constexpr bool PRINT_DEFAULT =
true;
51constexpr int32_t PRINT_FREQUENCY_MS_DEFAULT = 50;
52constexpr int32_t TIMEOUT_DEFAULT = -1;
55#if defined(__INTIME__)
56constexpr int32_t LOWEST_PRIORITY = 200;
57constexpr int32_t LOW_PRIORITY = 150;
58constexpr int32_t HIGH_PRIORITY = 0;
60constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
61constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
62constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
64constexpr int32_t LOWEST_PRIORITY = THREAD_PRIORITY_BELOW_NORMAL;
65constexpr int32_t LOW_PRIORITY = THREAD_PRIORITY_NORMAL;
66constexpr int32_t HIGH_PRIORITY = THREAD_PRIORITY_TIME_CRITICAL;
68constexpr int32_t SAMPLERATE_DEFAULT = Hz_1000;
69constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
70constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_250;
72constexpr int32_t LOWEST_PRIORITY = 0;
73constexpr int32_t LOW_PRIORITY = 5;
74constexpr int32_t HIGH_PRIORITY = 35;
75constexpr int32_t HIGH_BUT_NOT_TOO_HIGH_LINUX_NICE = -5;
76constexpr int32_t RMP_NICE_LINUX = -19;
78constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
79constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
80constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
82static const std::string& GetExePath()
84 static constexpr int PATH_MAX = 255;
85 static std::string exe_string;
86 if (exe_string.empty())
89 ssize_t len = ::readlink(
"/proc/self/exe", buf,
sizeof(buf));
90 if (len == -1 || len ==
sizeof(buf))
93 std::string buf_string(buf);
94 exe_string = buf_string.substr(0, buf_string.find_last_of(
"\\/") + 1);
106uint32_t cpuFrequency;
107int32_t currentPerformanceCounter;
108int32_t previousPerformanceCounter;
109int32_t deltaPerformanceCounter;
110int32_t syncInterruptIterations;
111double deltaMicroseconds;
112int32_t syncInterruptSampleCounter;
113int32_t lastSyncInterruptSampleCounter;
116std::atomic_bool readyToCleanup;
121int32_t printFrequencyMS;
124int32_t syncPeriodThreshold_uS;
127#if !defined(RSI_TEST)
129class BufferedDataImpl :
public StatisticsBuffer
133 BufferedDataImpl() : StatisticsBuffer() { this->Reset(); }
134 virtual void Init()
override { this->Reset(); }
135 virtual void Reset()
override { mean = 0; count = 0; }
136 virtual void AddData(
const double& datum)
override
138 this->StatisticsBuffer::AddData(datum);
140 double delta = datum - this->mean;
141 this->mean += delta / (++this->count);
145BufferedDataImpl buffered_data_impl;
146StatisticsBuffer& buffered_stats = buffered_data_impl;
148void PrintTimingInfo()
152 if (print && syncInterruptIterations)
154 printf(
"\t\t%ld\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\r",
155 syncInterruptIterations, deltaMicroseconds,
156 buffered_data_impl.min, buffered_data_impl.max, buffered_data_impl.mean);
159void printTimingHeaderString()
161 printf(
"Number Processed\t|\tDeltaT (uS)\t|\tMin (uS)\t|\tMax (uS)\t|\tMean (uS)\n");
164void StatisticsThread() {
return; }
167void SyncInterruptMainLoopThread()
169 const double cpuPeriod = 1.0 / cpuFrequency;
170 const double cpuPeriod_uS = cpuPeriod * MICROSECONDS_PER_SECOND;
171 double sample_frequency = 1.0 / sampleRate;
177 double sync_period_us = syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND;
179 double threshold_low_us, threshold_high_us;
180 threshold_low_us = sync_period_us - syncPeriodThreshold_uS;
181 threshold_high_us = sync_period_us + syncPeriodThreshold_uS;
182 printf(
"Threshold Set [%8.1lf %8.1lf]\n", threshold_low_us, threshold_high_us);
183 printf(
"Sync Period Set %i (~%.1lf us).\n", syncPeriod, syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND);
185 buffered_stats.Init();
198 while (!readyToCleanup)
215 deltaPerformanceCounter = currentPerformanceCounter - previousPerformanceCounter;
216 deltaMicroseconds = deltaPerformanceCounter * cpuPeriod_uS;
219 buffered_stats.AddData(deltaMicroseconds);
222 if (deltaMicroseconds < threshold_low_us || threshold_high_us < deltaMicroseconds)
225 printf(
"Sync Interrupt exceeded range of [%8.1lf %8.1lf] : %8.1lf\n", threshold_low_us, threshold_high_us, deltaMicroseconds);
228 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
229 readyToCleanup =
true;
233 if (syncInterruptSampleCounter == lastSyncInterruptSampleCounter)
236 printf(
"Sync Interrupt Got a double sample. syncCounter %ld, lastSyncCounter %ld, deltaT %8.1lf\n",
237 syncInterruptSampleCounter, lastSyncInterruptSampleCounter, deltaMicroseconds);
240 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
241 readyToCleanup =
true;
246 if (syncInterruptSampleCounter != (lastSyncInterruptSampleCounter + syncPeriod))
249 printf(
"Sync Interrupt missed a sample. syncCounter %ld, lastSyncCounter %ld\n", syncInterruptSampleCounter, lastSyncInterruptSampleCounter);
252 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
253 readyToCleanup =
true;
265 previousPerformanceCounter = currentPerformanceCounter;
266 lastSyncInterruptSampleCounter = syncInterruptSampleCounter;
267 ++syncInterruptIterations;
273 buffered_stats.Reset();
280 while (!readyToCleanup && syncInterruptIterations == 0)
282 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
284 printTimingHeaderString();
287 std::this_thread::sleep_for(std::chrono::milliseconds(printFrequencyMS));
289 }
while (!readyToCleanup);
292void KeyPressExitThread()
295 while (controller->
OS->
KeyGet((int32_t)RSIWait::RSIWaitPOLL) < 0 && !readyToCleanup)
297 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
299 readyToCleanup =
true;
308 std::chrono::milliseconds chrono_timeout(timeout_mS);
309 std::chrono::time_point start = std::chrono::high_resolution_clock::now();
310 std::chrono::nanoseconds duration;
313 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
314 duration = std::chrono::high_resolution_clock::now() - start;
315 }
while (duration < chrono_timeout && !readyToCleanup);
317 readyToCleanup =
true;
322void SystemEventHandlerThread()
325 EVENTINFO intimeEventInfo;
327 while (!RtNotifyEvent(RT_SYSTEM_NOTIFICATIONS | RT_EXIT_NOTIFICATIONS,
328 SLOW_LOOP_MILLISECONDS, &intimeEventInfo) && !readyToCleanup)
330 if (GetLastRtError())
335 switch (intimeEventInfo.dwNotifyType)
338 case NT_HOST_SHUTDOWN_PENDING:
339 case KERNEL_STOPPING:
340 case KERNEL_SHUTDOWN_PENDING:
346 readyToCleanup =
true;
362void RaiseProcessPriorityClass(
int cpuAffinity,
const char*
const threadName)
364#if defined(__INTIME__)
365#elif defined (_WIN32)
367 HANDLE hProcess = GetCurrentProcess();
370 dwPriClass = GetPriorityClass(hProcess);
371 _tprintf(TEXT(
"Current priority class is 0x%x\n"), dwPriClass);
373 if (!SetPriorityClass(hProcess, REALTIME_PRIORITY_CLASS))
375 _tprintf(TEXT(
"Failed to set to REALTIME_PRIORITY_CLASS (%d)\n"), GetLastError());
379 dwPriClass = GetPriorityClass(hProcess);
380 _tprintf(TEXT(
"Current priority class is 0x%x\n"), dwPriClass);
382 DWORD_PTR affinityMask = 1 << cpuAffinity;
383 BOOL success = SetProcessAffinityMask(hProcess, affinityMask);
385 wchar_t threadNameW[512];
386 swprintf_s(threadNameW, 512, L
"%S", threadName);
388 SetThreadDescription(GetCurrentThread(), threadNameW);
391 auto native_thread = pthread_self();
393 struct sched_param thread_schedule;
394 thread_schedule.sched_priority = LOW_PRIORITY;
395 pthread_setschedparam(native_thread, SCHED_OTHER, &thread_schedule);
397 cpu_set_t affinityMask;
398 CPU_ZERO(&affinityMask);
399 CPU_SET(cpuAffinity, &affinityMask);
400 if (pthread_setaffinity_np(native_thread,
sizeof(affinityMask), &affinityMask))
402 printf(
"Failed to set CPU affinity with errno %i", errno);
408 pthread_setname_np(native_thread, threadName);
413std::thread CreateThreadAndSetPriority(_Fp&& __f, int32_t priority,
const char*
const threadName)
415 std::thread thread = std::thread(__f);
416 auto native_thread = thread.native_handle();
421 SetRtThreadPriority(
reinterpret_cast<RTHANDLE
>(native_thread), priority);
426 SetThreadPriority(native_thread, priority);
427 int actualPriority = GetThreadPriority(native_thread);
429 std::stringstream id_ss;
430 id_ss << std::this_thread::get_id();
431 std::string tid_string = id_ss.str();
433 printf(
"Tried to set thread %s to priority %i. Is Actually %i\n",
434 tid_string.c_str(), priority, actualPriority
437 wchar_t threadNameW[512];
438 swprintf_s(threadNameW, 512, L
"%S", threadName);
440 SetThreadDescription(native_thread, threadNameW);
443 struct sched_param thread_schedule;
444 thread_schedule.sched_priority = priority;
456 pthread_setschedparam(native_thread, sched, &thread_schedule);
457 pthread_setname_np(native_thread, threadName);
465 print = PRINT_DEFAULT;
466 timeout_mS = TIMEOUT_DEFAULT;
467 printFrequencyMS = PRINT_FREQUENCY_MS_DEFAULT;
469 sampleRate = SAMPLERATE_DEFAULT;
470 syncPeriod = SYNCPERIOD_DEFAULT;
471 syncPeriodThreshold_uS = SYNCPERIODTHRESHOLD_DEFAULT;
474 DWORD_PTR dwProcessAffinity, dwSystemAffinity;
475 GetProcessAffinityMask(GetCurrentProcess(), &dwProcessAffinity, &dwSystemAffinity);
476 int64_t cpu_count = std::bitset<64>(dwProcessAffinity).count();
477 process_cpu =
static_cast<decltype(process_cpu)
>(cpu_count);
482 cpu_set_t affinityMask;
483 CPU_ZERO(&affinityMask);
484 sched_getaffinity(0,
sizeof(affinityMask), &affinityMask);
485 int32_t cpu_count = CPU_COUNT(&affinityMask);
487 process_cpu = rmp_cpu - 1;
490 const auto rmpPath = GetExePath();
495 const char* strarg_samplerate =
"-SampleRate";
496 const char* strarg_syncperiod =
"-SyncPeriod";
497 const char* strarg_printfreq =
"-PrintFrequencyMS";
498 const char* strarg_timeoutms =
"-Timeoutms";
499 const char* strarg_print =
"-Print";
500 const char* strarg_thresholdus =
"-Thresholdus";
501 const char* strarg_rmppath =
"-RmpPath";
502 for (
int i = 1; i < argc; ++i)
504 if (std::strncmp(argv[i], strarg_samplerate,
sizeof(strarg_samplerate)) == 0)
506 if ((i + 1) < argc && argv[i+1][0] !=
'-')
508 sampleRate = strtol(argv[++i],
nullptr, 10);
511 else if (std::strncmp(argv[i], strarg_syncperiod,
sizeof(strarg_syncperiod)) == 0)
513 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
515 syncPeriod = strtol(argv[++i],
nullptr, 10);
518 else if (std::strncmp(argv[i], strarg_printfreq,
sizeof(strarg_printfreq)) == 0)
520 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
522 printFrequencyMS = strtol(argv[++i],
nullptr, 10);
525 else if (std::strncmp(argv[i], strarg_timeoutms,
sizeof(strarg_timeoutms)) == 0)
529 timeout_mS = strtol(argv[++i],
nullptr, 10);
532 else if (std::strncmp(argv[i], strarg_print,
sizeof(strarg_print)) == 0)
534 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
536 char* bVal = argv[++i];
537 if (std::strncmp(bVal,
"t",
sizeof(
"t")) || std::strncmp(bVal,
"true",
sizeof(
"true")))
541 else if (std::strncmp(bVal,
"f",
sizeof(
"f")) || std::strncmp(bVal,
"false",
sizeof(
"false")))
551 else if (std::strncmp(argv[i], strarg_thresholdus,
sizeof(strarg_thresholdus)) == 0)
553 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
555 int parsed_val = strtol(argv[++i],
nullptr, 10);
558 syncPeriodThreshold_uS = parsed_val;
562 else if (std::strncmp(argv[i], strarg_rmppath,
sizeof(strarg_rmppath)) == 0)
566 std::string newRmpPath(argv[++i]);
575int main(
int argc,
char* argv[])
577int32_t SyncInterruptMain(int32_t argc,
char* argv[])
581 ParseSetGlobalArgs(argc, argv, params);
584 previousPerformanceCounter = 0;
585 syncInterruptIterations = 0;
590 printf(
"Hello, RapidCodeRT!\n");
594 std::cout <<
"params.rmpPath: " << params.
RmpPath << std::endl;
601 printf(
"Sample Rate: %8.0lf \n", controller->
SampleRateGet());
608 printf(
"CPU Frequency is: %u Hz\n", cpuFrequency);
611 std::vector<std::thread> threads;
614 RaiseProcessPriorityClass(process_cpu,
"SyncInterruptMainThread");
617 readyToCleanup =
false;
618 threads.push_back(CreateThreadAndSetPriority(&SystemEventHandlerThread, LOWEST_PRIORITY,
"SystemEventThread"));
619 threads.push_back(CreateThreadAndSetPriority(&TimeoutThread, LOWEST_PRIORITY,
"TimeoutThread"));
620 threads.push_back(CreateThreadAndSetPriority(&KeyPressExitThread, LOWEST_PRIORITY,
"KeypressThread"));
621 threads.push_back(CreateThreadAndSetPriority(&PrinterThread, LOW_PRIORITY,
"PrinterThread"));
622 threads.push_back(CreateThreadAndSetPriority(&StatisticsThread, LOW_PRIORITY,
"StatisticsThread"));
625 threads.push_back(CreateThreadAndSetPriority(&SyncInterruptMainLoopThread, HIGH_PRIORITY,
"SyncInterruptThread"));
628 for (
auto& thread : threads)
630 if (thread.joinable())
637 if (controller !=
nullptr)
643 catch (std::exception
const& e)
645 printf(
"\n%s\n", e.what());
static void CheckErrors(RapidCodeObject *rsiObject)
Checks for errors in the given RapidCodeObject and throws an exception if any non-warning errors are ...
void SampleRateSet(double sampleRate)
void ServiceThreadEnableSet(bool enable)
Enable or disable the service thread.
void SyncInterruptEnableSet(bool enable)
Configure Sync (periodic) interrupts for the controller.
static MotionController * Create()
Initialize and start the RMP EtherCAT controller.
void SyncInterruptPeriodSet(uint32_t samples)
Configure the period for the Sync Interrupt on the controller.
void Delete(void)
Delete the MotionController and all its objects.
int32_t SyncInterruptWait()
Suspend the current thread until an interrupt arrives from the controller.
uint32_t SerialNumberGet(void)
Get the controller's serial number.
Represents the RMP soft motion controller. This class provides an interface to general controller con...
RapidCodeOS * OS
Provides access to operating system (Windows) features.
int32_t KeyGet(int32_t milliseconds)
Wait for a key to be pressed and return its value.
int32_t PerformanceTimerFrequencyGet()
Gets the frequency of the performance counter.
int32_t PerformanceTimerCountGet()
Gets the current high performance counter value.
char RmpPath[PathLengthMaximum]
Location of the RMP firmware executable, license, and associated binaries and resources.
int32_t CpuAffinity
[Linux] Indicate the CPU core on which the RMP and RMPNetwork processes run.
static constexpr uint32_t PathLengthMaximum
MotionController::CreationParameters Maximum string buffer length.
CreationParameters for MotionController::Create.