APIs, concepts, guides, and more
Sync Interrupt

Sync Interrupt sample application.

Use a periodic interrupt from the controller. (Requires INtime RTOS and RMP RapidCodeRT license)

Precondition
This sample code presumes that the user has set the tuning paramters(PID, PIV, etc.) prior to running this program so that the motor can rotate in a stable manner.
Warning
This is a sample program to assist in the integration of the RMP motion controller with your application. It may not contain all of the logic and safety features that your application requires.
#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:794
RapidCodeOS * OS
Provides access to operating system (Windows) features.
Definition rsi.h:3664
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:938
int32_t CpuAffinity
[Linux] Indicate the CPU core on which the RMP and RMPNetwork processes run.
Definition rsi.h:990
static constexpr uint32_t PathLengthMaximum
MotionController::CreationParameters Maximum string buffer length.
Definition rsi.h:865
CreationParameters for MotionController::Create.
Definition rsi.h:855