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 "HelperFunctions.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, RSI::RapidCode::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());
      }
    }
  }
}
 
int32_t main(int32_t argc, char* argv[])
{
  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);
    HelperFunctions::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;
}