#ifdef __INTIME__
#define NOMINMAX
#include "rt.h"
#elif _WIN32
#define NOMINMAX
#include "Windows.h"
#include <tchar.h>
#include <bitset>
#else
#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"
constexpr int32_t SYNC_INTERRUPT_EXIT_ERROR = 5;
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
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
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;
int32_t sampleRate;
int32_t syncPeriod;
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()
{
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;
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();
while (!readyToCleanup)
{
deltaPerformanceCounter = currentPerformanceCounter - previousPerformanceCounter;
deltaMicroseconds = deltaPerformanceCounter * cpuPeriod_uS;
buffered_stats.AddData(deltaMicroseconds);
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;
}
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;
}
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;
}
previousPerformanceCounter = currentPerformanceCounter;
lastSyncInterruptSampleCounter = syncInterruptSampleCounter;
++syncInterruptIterations;
}
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()
{
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;
return;
}
void SystemEventHandlerThread()
{
#if __INTIME__
EVENTINFO intimeEventInfo;
while (!RtNotifyEvent(RT_SYSTEM_NOTIFICATIONS | RT_EXIT_NOTIFICATIONS,
SLOW_LOOP_MILLISECONDS, &intimeEventInfo) && !readyToCleanup)
{
if (GetLastRtError())
{
continue;
}
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;
}
void RaiseProcessPriorityClass(int cpuAffinity, const char* const threadName)
{
#if defined(__INTIME__)
#elif defined (_WIN32)
DWORD dwPriClass;
HANDLE hProcess = GetCurrentProcess();
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());
}
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
auto native_thread = pthread_self();
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);
}
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__
SetRtThreadPriority(reinterpret_cast<RTHANDLE>(native_thread), priority);
#elif _WIN32
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
struct sched_param thread_schedule;
thread_schedule.sched_priority = priority;
int sched;
switch(priority)
{
case HIGH_PRIORITY:
sched = SCHED_FIFO;
break;
default:
sched = SCHED_OTHER;
break;
}
pthread_setschedparam(native_thread, sched, &thread_schedule);
pthread_setname_np(native_thread, threadName);
#endif
return thread;
}
{
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
cpu_set_t affinityMask;
CPU_ZERO(&affinityMask);
sched_getaffinity(0, sizeof(affinityMask), &affinityMask);
int32_t cpu_count = CPU_COUNT(&affinityMask);
int32_t rmp_cpu = 3;
process_cpu = rmp_cpu - 1;
const auto rmpPath = GetExePath();
std::snprintf(params.
RmpPath, MotionController::CreationParameters::PathLengthMaximum, rmpPath.c_str());
#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
{
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]);
}
}
}
}
#ifdef __linux__
int main(int argc, char* argv[])
#else
int32_t SyncInterruptMain(int32_t argc, char* argv[])
#endif
{
ParseSetGlobalArgs(argc, argv, params);
previousPerformanceCounter = 0;
syncInterruptIterations = 0;
returnCode = 0;
try
{
printf("Hello, RapidCodeRT!\n");
std::cout <<
"params.rmpPath: " << params.
RmpPath << std::endl;
controller = MotionController::Create(¶ms);
printf("CPU Frequency is: %u Hz\n", cpuFrequency);
std::vector<std::thread> threads;
RaiseProcessPriorityClass(process_cpu, "SyncInterruptMainThread");
readyToCleanup = false;
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"));
threads.push_back(CreateThreadAndSetPriority(&SyncInterruptMainLoopThread, HIGH_PRIORITY, "SyncInterruptThread"));
for (auto& thread : threads)
{
if (thread.joinable())
{
thread.join();
}
}
printf("\n");
if (controller != nullptr)
{
}
}
catch (std::exception const& e)
{
printf("\n%s\n", e.what());
}
return returnCode;
}
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.
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.