The RMP Motion Controller APIs
SyncInterrupt.cpp
#ifdef __INTIME__ // INTIME!
#define NOMINMAX
#include "rt.h"
#elif _WIN32 // Windows
#define NOMINMAX
#include "Windows.h"
#include <tchar.h>
#include <bitset>
#else // linux
#include <unistd.h>
#include <sys/resource.h>
#endif //
#include <inttypes.h>
#include <vector>
#include <thread>
#include <stdio.h>
#include <sstream>
#include <atomic>
#include <cstring>
#include <cstdio>
#include "rsi.h"
#include "SampleAppsHelper.h"
#include "SyncInterrupt.h"
using namespace RSI::RapidCode; // Import the RapidCode namespace
// Changeable Constants
constexpr int32_t SYNC_INTERRUPT_EXIT_ERROR = 5;
// thread priorities
constexpr int32_t SLOW_LOOP_MILLISECONDS = 50;
constexpr bool PRINT_DEFAULT = true;
constexpr int32_t PRINT_FREQUENCY_MS_DEFAULT = 50;
constexpr int32_t TIMEOUT_DEFAULT = -1;
#if defined(__INTIME__)
constexpr int32_t LOWEST_PRIORITY = 200;
constexpr int32_t LOW_PRIORITY = 150;
constexpr int32_t HIGH_PRIORITY = 0;
constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
#elif defined(_WIN32)
constexpr int32_t LOWEST_PRIORITY = THREAD_PRIORITY_BELOW_NORMAL;
constexpr int32_t LOW_PRIORITY = THREAD_PRIORITY_NORMAL;
constexpr int32_t HIGH_PRIORITY = THREAD_PRIORITY_TIME_CRITICAL;
constexpr int32_t SAMPLERATE_DEFAULT = Hz_1000;
constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_250;
#else // Linux
constexpr int32_t LOWEST_PRIORITY = 0;
constexpr int32_t LOW_PRIORITY = 5;
constexpr int32_t HIGH_PRIORITY = 35;
constexpr int32_t HIGH_BUT_NOT_TOO_HIGH_LINUX_NICE = -5;
constexpr int32_t RMP_NICE_LINUX = -19;
constexpr int32_t SAMPLERATE_DEFAULT = Hz_4000;
constexpr int32_t SYNCPERIOD_DEFAULT = sync_1;
constexpr int32_t SYNCPERIODTHRESHOLD_DEFAULT = threshold_65;
static const std::string& GetExePath()
{
static constexpr int PATH_MAX = 255;
static std::string exe_string;
if (exe_string.empty())
{
char buf[PATH_MAX];
ssize_t len = ::readlink("/proc/self/exe", buf, sizeof(buf));
if (len == -1 || len == sizeof(buf))
len = 0;
buf[len] = '\0';
std::string buf_string(buf);
exe_string = buf_string.substr(0, buf_string.find_last_of("\\/") + 1);
}
return exe_string;
}
#endif
// RapidCode objects
// Instantiate variables
// shared variables
uint32_t cpuFrequency;
int32_t currentPerformanceCounter;
int32_t previousPerformanceCounter;
int32_t deltaPerformanceCounter;
int32_t syncInterruptIterations;
double deltaMicroseconds;
int32_t syncInterruptSampleCounter;
int32_t lastSyncInterruptSampleCounter;
int32_t returnCode;
std::atomic_bool readyToCleanup;
// Configurable
int32_t sampleRate; // hz
int32_t syncPeriod; // interrupt every MotionController SYNC_PERIOD samples
int32_t printFrequencyMS;
bool print;
int32_t timeout_mS;
int32_t syncPeriodThreshold_uS;
int32_t process_cpu;
#if !defined(RSI_TEST)
class BufferedDataImpl : public StatisticsBuffer
{
public:
double mean, count;
BufferedDataImpl() : StatisticsBuffer() { this->Reset(); }
virtual void Init() override { this->Reset(); }
virtual void Reset() override { mean = 0; count = 0; }
virtual void AddData(const double& datum) override
{
this->StatisticsBuffer::AddData(datum);
double delta = datum - this->mean;
this->mean += delta / (++this->count);
}
};
BufferedDataImpl buffered_data_impl;
StatisticsBuffer& buffered_stats = buffered_data_impl;
void PrintTimingInfo()
{
// && iterations to wait until we start looping.
// short circuit on bPrint
if (print && syncInterruptIterations)
{
printf("\t\t%ld\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\t|\t%8.1lfus\r",
syncInterruptIterations, deltaMicroseconds,
buffered_data_impl.min, buffered_data_impl.max, buffered_data_impl.mean);
}
}
void printTimingHeaderString()
{
printf("Number Processed\t|\tDeltaT (uS)\t|\tMin (uS)\t|\tMax (uS)\t|\tMean (uS)\n");
}
void StatisticsThread() { return; }
#endif
void SyncInterruptMainLoopThread()
{
const double cpuPeriod = 1.0 / cpuFrequency;
const double cpuPeriod_uS = cpuPeriod * MICROSECONDS_PER_SECOND;
double sample_frequency = 1.0 / sampleRate; // seconds per sample
// sync_period conversions
// sample_frequency * 1e0 = seconds per sample
// sample_frequency * 1e3 = milliseconds per sample
// sample_frequency * 1e6 = microseconds per sample
double sync_period_us = syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND;
double threshold_low_us, threshold_high_us;
threshold_low_us = sync_period_us - syncPeriodThreshold_uS;
threshold_high_us = sync_period_us + syncPeriodThreshold_uS;
printf("Threshold Set [%8.1lf %8.1lf]\n", threshold_low_us, threshold_high_us);
printf("Sync Period Set %i (~%.1lf us).\n", syncPeriod, syncPeriod * sample_frequency * MICROSECONDS_PER_SECOND);
buffered_stats.Init();
// configure a Sync interrupt every syncPeriod samples
controller->SyncInterruptPeriodSet(syncPeriod);
// enable controller interrupts
controller->SyncInterruptEnableSet(true);
lastSyncInterruptSampleCounter = controller->SyncInterruptWait();
previousPerformanceCounter = controller->OS->PerformanceTimerCountGet();
// polling threads will set this false
while (!readyToCleanup)
{
// Wait for the interrupt
syncInterruptSampleCounter = controller->SyncInterruptWait();
// Calculate metrics and make sure that we are within our tolerances
// On all systems this should wake up within the sample of the interrupt.
// So the current sample should equal the last sample + our sync period
//
// On Real Time systems, the actual measured time between periods should be
// no greater than the period plus some defined maximum.
// This value is system specific.
// On non-RT systems, the actual measured time between periods should be on
// average the period, with most close to and some greater than the period.
// There is no maximum.
currentPerformanceCounter = controller->OS->PerformanceTimerCountGet();
deltaPerformanceCounter = currentPerformanceCounter - previousPerformanceCounter;
deltaMicroseconds = deltaPerformanceCounter * cpuPeriod_uS;
// add data to a rotating circular buffer for stats thread can calc
buffered_stats.AddData(deltaMicroseconds);
// Check if our absolute delta time (us) is within our threshold
if (deltaMicroseconds < threshold_low_us || threshold_high_us < deltaMicroseconds)
{
printf("\n");
printf("Sync Interrupt exceeded range of [%8.1lf %8.1lf] : %8.1lf\n", threshold_low_us, threshold_high_us, deltaMicroseconds);
PrintTimingInfo();
printf("\n");
returnCode = SYNC_INTERRUPT_EXIT_ERROR;
readyToCleanup = true;
break;
}
// check this before syncInterruptSampleCounter != (lastSyncInterruptSampleCounter + syncPeriod)
if (syncInterruptSampleCounter == lastSyncInterruptSampleCounter)
{
printf("\n");
printf("Sync Interrupt Got a double sample. syncCounter %ld, lastSyncCounter %ld, deltaT %8.1lf\n",
syncInterruptSampleCounter, lastSyncInterruptSampleCounter, deltaMicroseconds);
PrintTimingInfo();
printf("\n");
returnCode = SYNC_INTERRUPT_EXIT_ERROR;
readyToCleanup = true;
break;
}
// check if we were delayed between getting the interrupt and getting the sample counter
// Or if we completely missed a sample
if (syncInterruptSampleCounter != (lastSyncInterruptSampleCounter + syncPeriod))
{
printf("\n");
printf("Sync Interrupt missed a sample. syncCounter %ld, lastSyncCounter %ld\n", syncInterruptSampleCounter, lastSyncInterruptSampleCounter);
PrintTimingInfo();
printf("\n");
returnCode = SYNC_INTERRUPT_EXIT_ERROR;
readyToCleanup = true;
break;
}
// Do your calculations HERE!
// set current to last for next loop
previousPerformanceCounter = currentPerformanceCounter;
lastSyncInterruptSampleCounter = syncInterruptSampleCounter;
++syncInterruptIterations;
}
// turn off Sync Interrupt
controller->SyncInterruptEnableSet(false);
buffered_stats.Reset();
return;
}
void PrinterThread()
{
while (!readyToCleanup && syncInterruptIterations == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
}
printTimingHeaderString();
do
{
std::this_thread::sleep_for(std::chrono::milliseconds(printFrequencyMS));
PrintTimingInfo();
} while (!readyToCleanup);
}
void KeyPressExitThread()
{
// wait for someone to press a key
while (controller->OS->KeyGet((int32_t)RSIWait::RSIWaitPOLL) < 0 && !readyToCleanup)
{
std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
}
readyToCleanup = true;
}
void TimeoutThread()
{
if (timeout_mS < 0)
{
return;
}
std::chrono::milliseconds chrono_timeout(timeout_mS);
std::chrono::time_point start = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds duration;
do
{
std::this_thread::sleep_for(std::chrono::milliseconds(SLOW_LOOP_MILLISECONDS));
duration = std::chrono::high_resolution_clock::now() - start;
} while (duration < chrono_timeout && !readyToCleanup);
readyToCleanup = true; // set in case we timed out!
return;
}
void SystemEventHandlerThread()
{
#if __INTIME__
EVENTINFO intimeEventInfo;
// wait for notification and check
while (!RtNotifyEvent(RT_SYSTEM_NOTIFICATIONS | RT_EXIT_NOTIFICATIONS,
SLOW_LOOP_MILLISECONDS, &intimeEventInfo) && !readyToCleanup)
{
if (GetLastRtError())
{
continue;
} // else E_OK
switch (intimeEventInfo.dwNotifyType)
{
case TERMINATE:
case NT_HOST_SHUTDOWN_PENDING:
case KERNEL_STOPPING:
case KERNEL_SHUTDOWN_PENDING:
case RT_CLIENT_DOWN:
case RT_CLIENT_UP:
case NT_HOST_DOWN:
case NT_HOST_UP:
case NT_BLUESCREEN:
readyToCleanup = true;
break;
}
}
#elif _WIN32
#else
#endif
return;
}
// Raises the process base priority to realtime.
// If you successfully raise the priority to realtime...
// ALL THREADS RUN WITH A BASE PRIORITY OF RT.
void RaiseProcessPriorityClass(int cpuAffinity, const char* const threadName)
{
#if defined(__INTIME__)
#elif defined (_WIN32)
DWORD dwPriClass;
HANDLE hProcess = GetCurrentProcess();
// Display priority class
dwPriClass = GetPriorityClass(hProcess);
_tprintf(TEXT("Current priority class is 0x%x\n"), dwPriClass);
if (!SetPriorityClass(hProcess, REALTIME_PRIORITY_CLASS))
{
_tprintf(TEXT("Failed to set to REALTIME_PRIORITY_CLASS (%d)\n"), GetLastError());
}
// Display priority class
dwPriClass = GetPriorityClass(hProcess);
_tprintf(TEXT("Current priority class is 0x%x\n"), dwPriClass);
DWORD_PTR affinityMask = 1 << cpuAffinity;
BOOL success = SetProcessAffinityMask(hProcess, affinityMask);
wchar_t threadNameW[512];
swprintf_s(threadNameW, 512, L"%S", threadName);
SetThreadDescription(GetCurrentThread(), threadNameW);
#else // linux
auto native_thread = pthread_self();
// set this low for main
struct sched_param thread_schedule;
thread_schedule.sched_priority = LOW_PRIORITY;
pthread_setschedparam(native_thread, SCHED_OTHER, &thread_schedule);
cpu_set_t affinityMask;
CPU_ZERO(&affinityMask);
CPU_SET(cpuAffinity, &affinityMask);
if (pthread_setaffinity_np(native_thread, sizeof(affinityMask), &affinityMask))
{
printf("Failed to set CPU affinity with errno %i", errno);
}
//if (sched_setaffinity(getpid(), sizeof(affinityMask), &affinityMask))
//{
// printf("Failed to set CPU affinity with errno %i", errno);
//}
pthread_setname_np(native_thread, threadName);
#endif
}
template<class _Fp>
std::thread CreateThreadAndSetPriority(_Fp&& __f, int32_t priority, const char* const threadName)
{
std::thread thread = std::thread(__f);
auto native_thread = thread.native_handle();
#if __INTIME__
// std::thread puts the restult of CreateRtThread into std::thread::__t_ of type native_handle(=void*)
// std::thread::native_handle returns std::thread::__t_
//RTHANDLE native_thread = reinterpret_cast<RTHANDLE>(thread.native_handle());
SetRtThreadPriority(reinterpret_cast<RTHANDLE>(native_thread), priority);
#elif _WIN32
// crank up the thread priority
//HANDLE currentThreadHandle = thread.native_handle();
// Set the thread priority to time critical
SetThreadPriority(native_thread, priority);
int actualPriority = GetThreadPriority(native_thread);
std::stringstream id_ss;
id_ss << std::this_thread::get_id();
std::string tid_string = id_ss.str();
printf("Tried to set thread %s to priority %i. Is Actually %i\n",
tid_string.c_str(), priority, actualPriority
);
wchar_t threadNameW[512];
swprintf_s(threadNameW, 512, L"%S", threadName);
SetThreadDescription(native_thread, threadNameW);
#else
//pthread_t native_thread = thread.native_handle();
struct sched_param thread_schedule;
thread_schedule.sched_priority = priority;
int sched;
switch(priority)
{
case HIGH_PRIORITY:
sched = SCHED_FIFO;
break;
default: // LOWEST_PRIORITY, LOW_PRIORITY
sched = SCHED_OTHER;
break;
}
pthread_setschedparam(native_thread, sched, &thread_schedule);
pthread_setname_np(native_thread, threadName);
#endif
return thread;
}
void ParseSetGlobalArgs(int32_t argc, char* argv[], MotionController::CreationParameters &params)
{
print = PRINT_DEFAULT;
timeout_mS = TIMEOUT_DEFAULT;
printFrequencyMS = PRINT_FREQUENCY_MS_DEFAULT;
sampleRate = SAMPLERATE_DEFAULT;
syncPeriod = SYNCPERIOD_DEFAULT;
syncPeriodThreshold_uS = SYNCPERIODTHRESHOLD_DEFAULT;
#if __INTIME__
#elif _WIN32
DWORD_PTR dwProcessAffinity, dwSystemAffinity;
GetProcessAffinityMask(GetCurrentProcess(), &dwProcessAffinity, &dwSystemAffinity);
int64_t cpu_count = std::bitset<64>(dwProcessAffinity).count();
process_cpu = static_cast<decltype(process_cpu)>(cpu_count);
#else // Linux
// syncPeriodThreshold_uS = syncPeriod * (1.0e6 / sampleRate); // 1/hz = seconds -> us = *1e6
// we want to be able to use cat /sys/devices/system/cpu/online
// sched_getaffinity(0,...) only shows cpus available for the scheduler....
cpu_set_t affinityMask;
CPU_ZERO(&affinityMask);
sched_getaffinity(0, sizeof(affinityMask), &affinityMask);
int32_t cpu_count = CPU_COUNT(&affinityMask); // 0 indexed
int32_t rmp_cpu = 3;
process_cpu = rmp_cpu - 1;
params.CpuAffinity = rmp_cpu;
const auto rmpPath = GetExePath();
std::snprintf(params.RmpPath, MotionController::CreationParameters::PathLengthMaximum, rmpPath.c_str());
// params.nicePriority = RMP_NICE_LINUX;
#endif
const char* strarg_samplerate = "-SampleRate";
const char* strarg_syncperiod = "-SyncPeriod";
const char* strarg_printfreq = "-PrintFrequencyMS";
const char* strarg_timeoutms = "-Timeoutms";
const char* strarg_print = "-Print";
const char* strarg_thresholdus = "-Thresholdus";
const char* strarg_rmppath = "-RmpPath";
for (int i = 1; i < argc; ++i)
{
if (std::strncmp(argv[i], strarg_samplerate, sizeof(strarg_samplerate)) == 0)
{
if ((i + 1) < argc && argv[i+1][0] != '-')
{
sampleRate = strtol(argv[++i], nullptr, 10);
}
}
else if (std::strncmp(argv[i], strarg_syncperiod, sizeof(strarg_syncperiod)) == 0)
{
if ((i + 1) < argc && argv[i + 1][0] != '-')
{
syncPeriod = strtol(argv[++i], nullptr, 10);
}
}
else if (std::strncmp(argv[i], strarg_printfreq, sizeof(strarg_printfreq)) == 0)
{
if ((i + 1) < argc && argv[i + 1][0] != '-')
{
printFrequencyMS = strtol(argv[++i], nullptr, 10);
}
}
else if (std::strncmp(argv[i], strarg_timeoutms, sizeof(strarg_timeoutms)) == 0)
{
if ((i + 1) < argc)
{
timeout_mS = strtol(argv[++i], nullptr, 10);
}
}
else if (std::strncmp(argv[i], strarg_print, sizeof(strarg_print)) == 0)
{
if ((i + 1) < argc && argv[i + 1][0] != '-')
{
char* bVal = argv[++i];
if (std::strncmp(bVal, "t", sizeof("t")) || std::strncmp(bVal, "true", sizeof("true")))
{
print = true;
}
else if (std::strncmp(bVal, "f", sizeof("f")) || std::strncmp(bVal, "false", sizeof("false")))
{
print = true;
}
}
else // flag present and no t/f set
{
print = true;
}
}
else if (std::strncmp(argv[i], strarg_thresholdus, sizeof(strarg_thresholdus)) == 0)
{
if ((i + 1) < argc && argv[i + 1][0] != '-')
{
int parsed_val = strtol(argv[++i], nullptr, 10);
if (-1 < parsed_val)
{
syncPeriodThreshold_uS = parsed_val;
}
}
}
else if (std::strncmp(argv[i], strarg_rmppath, sizeof(strarg_rmppath)) == 0)
{
if ((i + 1) < argc)
{
std::string newRmpPath(argv[++i]);
}
}
}
}
// Necessary to build correctly on linux, as the SyncInterrupt is expected to be the entry point to SampleAppsCPP
#ifdef __linux__
int main(int argc, char* argv[])
#else
int32_t SyncInterruptMain(int32_t argc, char* argv[])
#endif
{
ParseSetGlobalArgs(argc, argv, params);
// Zero initialize variables
previousPerformanceCounter = 0;
syncInterruptIterations = 0;
returnCode = 0; // OK!
try
{
printf("Hello, RapidCodeRT!\n");
// create and Initialize MotionController class.
std::cout << "params.rmpPath: " << params.RmpPath << std::endl;
controller = MotionController::Create(&params);
printf("Serial Number: %d \n", controller->SerialNumberGet());
controller->SampleRateSet(sampleRate);
printf("Sample Rate: %8.0lf \n", controller->SampleRateGet());
// disable the service thread if using Controller Sync Interrupt
controller->ServiceThreadEnableSet(false);
// Get CPU frequency from Operating System performance counter
cpuFrequency = controller->OS->PerformanceTimerFrequencyGet();
printf("CPU Frequency is: %u Hz\n", cpuFrequency);
// start all threads
std::vector<std::thread> threads;
// raise the base priority of the process. Be careful with this...
RaiseProcessPriorityClass(process_cpu, "SyncInterruptMainThread");
// start 3 slow polling threads
readyToCleanup = false; // set because the pollers check this
threads.push_back(CreateThreadAndSetPriority(&SystemEventHandlerThread, LOWEST_PRIORITY, "SystemEventThread"));
threads.push_back(CreateThreadAndSetPriority(&TimeoutThread, LOWEST_PRIORITY, "TimeoutThread"));
threads.push_back(CreateThreadAndSetPriority(&KeyPressExitThread, LOWEST_PRIORITY, "KeypressThread"));
threads.push_back(CreateThreadAndSetPriority(&PrinterThread, LOW_PRIORITY, "PrinterThread"));
threads.push_back(CreateThreadAndSetPriority(&StatisticsThread, LOW_PRIORITY, "StatisticsThread"));
// run the high priority loop
threads.push_back(CreateThreadAndSetPriority(&SyncInterruptMainLoopThread, HIGH_PRIORITY, "SyncInterruptThread"));
// Wait for all of our working threads to finish!
for (auto& thread : threads)
{
if (thread.joinable())
{
thread.join();
}
}
printf("\n");//flush
if (controller != nullptr)
{
// Delete the controller to clean up all RapidCodeRT objects
controller->Delete();
}
}
catch (std::exception const& e)
{
printf("\n%s\n", e.what());
}
return returnCode;
}
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.
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...
Definition rsi.h:762
RapidCodeOS * OS
Provides access to operating system (Windows) features.
Definition rsi.h:3736
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.
Definition rsi.h:909
int32_t CpuAffinity
[Linux] Indicate the CPU core on which the RMP and RMPNetwork processes run.
Definition rsi.h:961
static constexpr uint32_t PathLengthMaximum
MotionController::CreationParameters Maximum string buffer length.
Definition rsi.h:836
CreationParameters for MotionController::Create.
Definition rsi.h:826