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 "HelperFunctionsCpp.h"
#include "SyncInterrupt.h"
 
// 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
RSI::RapidCode::MotionController* controller;
 
// 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]);
        std::snprintf(params.RmpPath, RSI::RapidCode::MotionController::CreationParameters::PathLengthMaximum, newRmpPath.c_str());
      }
    }
  }
}
 
// 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
{
  MotionController::CreationParameters params;
  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);
    HelperFunctionsCpp::CheckErrors(controller);
    
    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;
}