28#include <sys/resource.h>
42#include "SampleAppsHelper.h"
43#include "SyncInterrupt.h"
48constexpr int32_t SYNC_INTERRUPT_EXIT_ERROR = 5;
50constexpr int32_t SLOW_LOOP_MILLISECONDS = 50;
52constexpr bool PRINT_DEFAULT =
true;
53constexpr int32_t PRINT_FREQUENCY_MS_DEFAULT = 50;
54constexpr int32_t TIMEOUT_DEFAULT = -1;
57#if defined(__INTIME__)
58constexpr int32_t LOWEST_PRIORITY = 200;
59constexpr int32_t LOW_PRIORITY = 150;
60constexpr int32_t HIGH_PRIORITY = 0;
62constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
63constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
64constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
66constexpr int32_t LOWEST_PRIORITY = THREAD_PRIORITY_BELOW_NORMAL;
67constexpr int32_t LOW_PRIORITY = THREAD_PRIORITY_NORMAL;
68constexpr int32_t HIGH_PRIORITY = THREAD_PRIORITY_TIME_CRITICAL;
70constexpr int32_t SAMPLERATE_DEFAULT = Hz_1000;
71constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
72constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_250;
74constexpr int32_t LOWEST_PRIORITY = 0;
75constexpr int32_t LOW_PRIORITY = 5;
76constexpr int32_t HIGH_PRIORITY = 35;
77constexpr int32_t HIGH_BUT_NOT_TOO_HIGH_LINUX_NICE = -5;
78constexpr int32_t RMP_NICE_LINUX = -19;
80constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
81constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
82constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
84static const std::string& GetExePath()
86 static constexpr int PATH_MAX = 255;
87 static std::string exe_string;
88 if (exe_string.empty())
91 ssize_t len = ::readlink(
"/proc/self/exe", buf,
sizeof(buf));
92 if (len == -1 || len ==
sizeof(buf))
95 std::string buf_string(buf);
96 exe_string = buf_string.substr(0, buf_string.find_last_of(
"\\/") + 1);
108uint32_t cpuFrequency;
109int32_t currentPerformanceCounter;
110int32_t previousPerformanceCounter;
111int32_t deltaPerformanceCounter;
112int32_t syncInterruptIterations;
113double deltaMicroseconds;
114int32_t syncInterruptSampleCounter;
115int32_t lastSyncInterruptSampleCounter;
118std::atomic_bool readyToCleanup;
123int32_t printFrequencyMS;
126int32_t syncPeriodThreshold_uS;
129#if !defined(RSI_TEST)
131class BufferedDataImpl :
public StatisticsBuffer
135 BufferedDataImpl() : StatisticsBuffer() { this->Reset(); }
136 virtual void Init()
override { this->Reset(); }
137 virtual void Reset()
override { mean = 0; count = 0; }
138 virtual void AddData(
const double& datum)
override
140 this->StatisticsBuffer::AddData(datum);
142 double delta = datum - this->mean;
143 this->mean += delta / (++this->count);
147BufferedDataImpl buffered_data_impl;
148StatisticsBuffer& buffered_stats = buffered_data_impl;
150void PrintTimingInfo()
154 if (print && syncInterruptIterations)
156 printf(
"\t\t%ld\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\r",
157 syncInterruptIterations, deltaMicroseconds,
158 buffered_data_impl.min, buffered_data_impl.max, buffered_data_impl.mean);
161void printTimingHeaderString()
163 printf(
"Number Processed\t|\tDeltaT (uS)\t|\tMin (uS)\t|\tMax (uS)\t|\tMean (uS)\n");
166void StatisticsThread() {
return; }
169void SyncInterruptMainLoopThread()
171 const double cpuPeriod = 1.0 / cpuFrequency;
172 const double cpuPeriod_uS = cpuPeriod * MICROSECONDS_PER_SECOND;
173 double sample_frequency = 1.0 / sampleRate;
179 double sync_period_us = syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND;
181 double threshold_low_us, threshold_high_us;
182 threshold_low_us = sync_period_us - syncPeriodThreshold_uS;
183 threshold_high_us = sync_period_us + syncPeriodThreshold_uS;
184 printf(
"Threshold Set [%8.1lf %8.1lf]\n", threshold_low_us, threshold_high_us);
185 printf(
"Sync Period Set %i (~%.1lf us).\n", syncPeriod, syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND);
187 buffered_stats.Init();
200 while (!readyToCleanup)
217 deltaPerformanceCounter = currentPerformanceCounter - previousPerformanceCounter;
218 deltaMicroseconds = deltaPerformanceCounter * cpuPeriod_uS;
221 buffered_stats.AddData(deltaMicroseconds);
224 if (deltaMicroseconds < threshold_low_us || threshold_high_us < deltaMicroseconds)
227 printf(
"Sync Interrupt exceeded range of [%8.1lf %8.1lf] : %8.1lf\n", threshold_low_us, threshold_high_us, deltaMicroseconds);
230 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
231 readyToCleanup =
true;
235 if (syncInterruptSampleCounter == lastSyncInterruptSampleCounter)
238 printf(
"Sync Interrupt Got a double sample. syncCounter %ld, lastSyncCounter %ld, deltaT %8.1lf\n",
239 syncInterruptSampleCounter, lastSyncInterruptSampleCounter, deltaMicroseconds);
242 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
243 readyToCleanup =
true;
248 if (syncInterruptSampleCounter != (lastSyncInterruptSampleCounter + syncPeriod))
251 printf(
"Sync Interrupt missed a sample. syncCounter %ld, lastSyncCounter %ld\n", syncInterruptSampleCounter, lastSyncInterruptSampleCounter);
254 returnCode = SYNC_INTERRUPT_EXIT_ERROR;
255 readyToCleanup =
true;
267 previousPerformanceCounter = currentPerformanceCounter;
268 lastSyncInterruptSampleCounter = syncInterruptSampleCounter;
269 ++syncInterruptIterations;
275 buffered_stats.Reset();
282 while (!readyToCleanup && syncInterruptIterations == 0)
284 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
286 printTimingHeaderString();
289 std::this_thread::sleep_for(std::chrono::milliseconds(printFrequencyMS));
291 }
while (!readyToCleanup);
294void KeyPressExitThread()
297 while (controller->
OS->
KeyGet((int32_t)RSIWait::RSIWaitPOLL) < 0 && !readyToCleanup)
299 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
301 readyToCleanup =
true;
310 std::chrono::milliseconds chrono_timeout(timeout_mS);
311 std::chrono::time_point start = std::chrono::high_resolution_clock::now();
312 std::chrono::nanoseconds duration;
315 std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
316 duration = std::chrono::high_resolution_clock::now() - start;
317 }
while (duration < chrono_timeout && !readyToCleanup);
319 readyToCleanup =
true;
324void SystemEventHandlerThread()
327 EVENTINFO intimeEventInfo;
329 while (!RtNotifyEvent(RT_SYSTEM_NOTIFICATIONS | RT_EXIT_NOTIFICATIONS,
330 SLOW_LOOP_MILLISECONDS, &intimeEventInfo) && !readyToCleanup)
332 if (GetLastRtError())
337 switch (intimeEventInfo.dwNotifyType)
340 case NT_HOST_SHUTDOWN_PENDING:
341 case KERNEL_STOPPING:
342 case KERNEL_SHUTDOWN_PENDING:
348 readyToCleanup =
true;
364void RaiseProcessPriorityClass(
int cpuAffinity,
const char*
const threadName)
366#if defined(__INTIME__)
367#elif defined (_WIN32)
369 HANDLE hProcess = GetCurrentProcess();
372 dwPriClass = GetPriorityClass(hProcess);
373 _tprintf(TEXT(
"Current priority class is 0x%x\n"), dwPriClass);
375 if (!SetPriorityClass(hProcess, REALTIME_PRIORITY_CLASS))
377 _tprintf(TEXT(
"Failed to set to REALTIME_PRIORITY_CLASS (%d)\n"), GetLastError());
381 dwPriClass = GetPriorityClass(hProcess);
382 _tprintf(TEXT(
"Current priority class is 0x%x\n"), dwPriClass);
384 DWORD_PTR affinityMask = 1 << cpuAffinity;
385 BOOL success = SetProcessAffinityMask(hProcess, affinityMask);
387 wchar_t threadNameW[512];
388 swprintf_s(threadNameW, 512, L
"%S", threadName);
390 SetThreadDescription(GetCurrentThread(), threadNameW);
393 auto native_thread = pthread_self();
395 struct sched_param thread_schedule;
396 thread_schedule.sched_priority = LOW_PRIORITY;
397 pthread_setschedparam(native_thread, SCHED_OTHER, &thread_schedule);
399 cpu_set_t affinityMask;
400 CPU_ZERO(&affinityMask);
401 CPU_SET(cpuAffinity, &affinityMask);
402 if (pthread_setaffinity_np(native_thread,
sizeof(affinityMask), &affinityMask))
404 printf(
"Failed to set CPU affinity with errno %i", errno);
410 pthread_setname_np(native_thread, threadName);
415std::thread CreateThreadAndSetPriority(_Fp&& __f, int32_t priority,
const char*
const threadName)
417 std::thread thread = std::thread(__f);
418 auto native_thread = thread.native_handle();
423 SetRtThreadPriority(
reinterpret_cast<RTHANDLE
>(native_thread), priority);
428 SetThreadPriority(native_thread, priority);
429 int actualPriority = GetThreadPriority(native_thread);
431 std::stringstream id_ss;
432 id_ss << std::this_thread::get_id();
433 std::string tid_string = id_ss.str();
435 printf(
"Tried to set thread %s to priority %i. Is Actually %i\n",
436 tid_string.c_str(), priority, actualPriority
439 wchar_t threadNameW[512];
440 swprintf_s(threadNameW, 512, L
"%S", threadName);
442 SetThreadDescription(native_thread, threadNameW);
445 struct sched_param thread_schedule;
446 thread_schedule.sched_priority = priority;
458 pthread_setschedparam(native_thread, sched, &thread_schedule);
459 pthread_setname_np(native_thread, threadName);
467 print = PRINT_DEFAULT;
468 timeout_mS = TIMEOUT_DEFAULT;
469 printFrequencyMS = PRINT_FREQUENCY_MS_DEFAULT;
471 sampleRate = SAMPLERATE_DEFAULT;
472 syncPeriod = SYNCPERIOD_DEFAULT;
473 syncPeriodThreshold_uS = SYNCPERIODTHRESHOLD_DEFAULT;
476 DWORD_PTR dwProcessAffinity, dwSystemAffinity;
477 GetProcessAffinityMask(GetCurrentProcess(), &dwProcessAffinity, &dwSystemAffinity);
478 int64_t cpu_count = std::bitset<64>(dwProcessAffinity).count();
479 process_cpu =
static_cast<decltype(process_cpu)
>(cpu_count);
484 cpu_set_t affinityMask;
485 CPU_ZERO(&affinityMask);
486 sched_getaffinity(0,
sizeof(affinityMask), &affinityMask);
487 int32_t cpu_count = CPU_COUNT(&affinityMask);
489 process_cpu = rmp_cpu - 1;
492 const auto rmpPath = GetExePath();
497 const char* strarg_samplerate =
"-SampleRate";
498 const char* strarg_syncperiod =
"-SyncPeriod";
499 const char* strarg_printfreq =
"-PrintFrequencyMS";
500 const char* strarg_timeoutms =
"-Timeoutms";
501 const char* strarg_print =
"-Print";
502 const char* strarg_thresholdus =
"-Thresholdus";
503 const char* strarg_rmppath =
"-RmpPath";
504 for (
int i = 1; i < argc; ++i)
506 if (std::strncmp(argv[i], strarg_samplerate,
sizeof(strarg_samplerate)) == 0)
508 if ((i + 1) < argc && argv[i+1][0] !=
'-')
510 sampleRate = strtol(argv[++i],
nullptr, 10);
513 else if (std::strncmp(argv[i], strarg_syncperiod,
sizeof(strarg_syncperiod)) == 0)
515 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
517 syncPeriod = strtol(argv[++i],
nullptr, 10);
520 else if (std::strncmp(argv[i], strarg_printfreq,
sizeof(strarg_printfreq)) == 0)
522 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
524 printFrequencyMS = strtol(argv[++i],
nullptr, 10);
527 else if (std::strncmp(argv[i], strarg_timeoutms,
sizeof(strarg_timeoutms)) == 0)
531 timeout_mS = strtol(argv[++i],
nullptr, 10);
534 else if (std::strncmp(argv[i], strarg_print,
sizeof(strarg_print)) == 0)
536 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
538 char* bVal = argv[++i];
539 if (std::strncmp(bVal,
"t",
sizeof(
"t")) || std::strncmp(bVal,
"true",
sizeof(
"true")))
543 else if (std::strncmp(bVal,
"f",
sizeof(
"f")) || std::strncmp(bVal,
"false",
sizeof(
"false")))
553 else if (std::strncmp(argv[i], strarg_thresholdus,
sizeof(strarg_thresholdus)) == 0)
555 if ((i + 1) < argc && argv[i + 1][0] !=
'-')
557 int parsed_val = strtol(argv[++i],
nullptr, 10);
560 syncPeriodThreshold_uS = parsed_val;
564 else if (std::strncmp(argv[i], strarg_rmppath,
sizeof(strarg_rmppath)) == 0)
568 std::string newRmpPath(argv[++i]);
577int main(
int argc,
char* argv[])
579int32_t SyncInterruptMain(int32_t argc,
char* argv[])
583 ParseSetGlobalArgs(argc, argv, params);
586 previousPerformanceCounter = 0;
587 syncInterruptIterations = 0;
592 printf(
"Hello, RapidCodeRT!\n");
596 std::cout <<
"params.rmpPath: " << params.
RmpPath << std::endl;
603 printf(
"Sample Rate: %8.0lf \n", controller->
SampleRateGet());
610 printf(
"CPU Frequency is: %u Hz\n", cpuFrequency);
613 std::vector<std::thread> threads;
616 RaiseProcessPriorityClass(process_cpu,
"SyncInterruptMainThread");
619 readyToCleanup =
false;
620 threads.push_back(CreateThreadAndSetPriority(&SystemEventHandlerThread, LOWEST_PRIORITY,
"SystemEventThread"));
621 threads.push_back(CreateThreadAndSetPriority(&TimeoutThread, LOWEST_PRIORITY,
"TimeoutThread"));
622 threads.push_back(CreateThreadAndSetPriority(&KeyPressExitThread, LOWEST_PRIORITY,
"KeypressThread"));
623 threads.push_back(CreateThreadAndSetPriority(&PrinterThread, LOW_PRIORITY,
"PrinterThread"));
624 threads.push_back(CreateThreadAndSetPriority(&StatisticsThread, LOW_PRIORITY,
"StatisticsThread"));
627 threads.push_back(CreateThreadAndSetPriority(&SyncInterruptMainLoopThread, HIGH_PRIORITY,
"SyncInterruptThread"));
630 for (
auto& thread : threads)
632 if (thread.joinable())
639 if (controller !=
nullptr)
645 catch (std::exception
const& e)
647 printf(
"\n%s\n", e.what());
void SampleRateSet(double sampleRate)
Configure the RMP firmware control loop frequency.
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.
static void CheckErrors(RapidCodeObject *rsiObject)
Checks for errors in the given RapidCodeObject and throws an exception if any non-warning errors are ...
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.