Adds new Threads API for priority setting while using std::thread (#379)

Also adds Java API for doing the same to Java threads (standard Java Thread.setPriority only works
when Java is run as root, even if the process has the right privileges).

hal/include/HAL/Errors.h

@@ -87,6 +87,14 @@
 #define HAL_SERIAL_PORT_NOT_FOUND_MESSAGE \
   "HAL: The specified serial port device was not found";
 
+#define HAL_THREAD_PRIORITY_ERROR -1152
+#define HAL_THREAD_PRIORITY_ERROR_MESSAGE \
+  "HAL: Getting or setting the priority of a thread has failed";
+
+#define HAL_THREAD_PRIORITY_RANGE_ERROR -1153
+#define HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE \
+  "HAL: The priority requested to be set is invalid"
+
 #define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
 #define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
 #define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"

hal/include/HAL/Threads.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "HAL/Types.h"
+
+#ifdef _WIN32
+#include <windows.h>
+#define NativeThreadHandle const HANDLE*
+#else
+#include <pthread.h>
+#define NativeThreadHandle const pthread_t*
+#endif
+
+int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
+                              int32_t* status);
+int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status);
+HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
+                               int32_t priority, int32_t* status);
+HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
+                                      int32_t* status);

hal/lib/athena/HAL.cpp

@@ -163,6 +163,10 @@ const char* HAL_GetErrorMessage(int32_t code) {
       return HAL_PWM_SCALE_ERROR_MESSAGE;
     case HAL_SERIAL_PORT_NOT_FOUND:
       return HAL_SERIAL_PORT_NOT_FOUND_MESSAGE;
+    case HAL_THREAD_PRIORITY_ERROR:
+      return HAL_THREAD_PRIORITY_ERROR_MESSAGE;
+    case HAL_THREAD_PRIORITY_RANGE_ERROR:
+      return HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE;
     default:
       return "Unknown error status";
   }

hal/lib/athena/Threads.cpp

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/Threads.h"
+
+#include <pthread.h>
+#include <sched.h>
+
+#include "HAL/Errors.h"
+
+/**
+ * Get the thread priority for the specified thread.
+ *
+ * @param handle Native handle pointer to the thread to get the priority for
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @param status Error status variable. 0 on success
+ * @return The current thread priority. Scaled 1-99, with 1 being highest.
+ */
+int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
+                              int32_t* status) {
+  sched_param sch;
+  int policy;
+  int success = pthread_getschedparam(*handle, &policy, &sch);
+  if (success == 0) {
+    *status = 0;
+  } else {
+    *status = HAL_THREAD_PRIORITY_ERROR;
+    return -1;
+  }
+  if (policy == SCHED_FIFO || policy == SCHED_RR) {
+    *isRealTime = true;
+    return sch.sched_priority;
+  } else {
+    *isRealTime = false;
+    // 0 is the only suppored priority for non-realtime, so scale to 1
+    return 1;
+  }
+}
+
+/**
+ * Get the thread priority for the current thread.
+ *
+ * @param handle Native handle pointer to the thread to get the priority for
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @param status Error status variable. 0 on success
+ * @return The current thread priority. Scaled 1-99, with 1 being highest.
+ */
+int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status) {
+  auto thread = pthread_self();
+  return HAL_GetThreadPriority(&thread, isRealTime, status);
+}
+
+/**
+ * Sets the thread priority for the specified thread
+ *
+ * @param thread Reference to the thread to set the priority of
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest
+ * @param status Error status variable. 0 on success
+ *
+ * @return The success state of setting the priority
+ */
+HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
+                               int32_t priority, int32_t* status) {
+  if (handle == nullptr) {
+    *status = NULL_PARAMETER;
+    return false;
+  }
+
+  int scheduler = realTime ? SCHED_FIFO : SCHED_OTHER;
+  if (realTime) {
+    // We don't support setting priorities for non RT threads
+    // so we don't need to check for proper range
+    if (priority < sched_get_priority_min(scheduler) ||
+        priority > sched_get_priority_max(scheduler)) {
+      *status = HAL_THREAD_PRIORITY_RANGE_ERROR;
+      return false;
+    }
+  }
+
+  sched_param sch;
+  int policy;
+  pthread_getschedparam(*handle, &policy, &sch);
+  if (scheduler == SCHED_FIFO || scheduler == SCHED_RR)
+    sch.sched_priority = priority;
+  else
+    // Only need to set 0 priority for non RT thread
+    sch.sched_priority = 0;
+  if (pthread_setschedparam(*handle, scheduler, &sch)) {
+    *status = HAL_THREAD_PRIORITY_ERROR;
+    return true;
+  } else {
+    *status = 0;
+    return false;
+  }
+}
+
+/**
+ * Sets the thread priority for the current thread
+ *
+ * @param thread Reference to the thread to set the priority of
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest
+ * @param status Error status variable. 0 on success
+ *
+ * @return The success state of setting the priority
+ */
+HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
+                                      int32_t* status) {
+  auto thread = pthread_self();
+  return HAL_SetThreadPriority(&thread, realTime, priority, status);
+}

wpilibc/athena/include/Task.h

@@ -20,8 +20,8 @@ namespace frc {
  * Wrapper class around std::thread that allows changing thread priority
  */
 class WPI_DEPRECATED(
-    "Task API scheduled for removal in 2018. Replace with std::thread") Task
-    : public ErrorBase {
+    "Task API scheduled for removal in 2018. Replace with std::thread, and use "
+    "Threads API for setting priority") Task : public ErrorBase {
  public:
   static const int kDefaultPriority = 60;
 

wpilibc/athena/include/Threads.h

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <thread>
+
+namespace frc {
+
+int GetThreadPriority(std::thread& thread, bool* isRealTime);
+int GetCurrentThreadPriority(bool* isRealTime);
+bool SetThreadPriority(std::thread& thread, bool realTime, int priority);
+bool SetCurrentThreadPriority(bool realTime, int priority);
+}  // namespace frc

wpilibc/athena/include/WPILib.h

@@ -76,6 +76,7 @@
 #include "SpeedController.h"
 #include "Talon.h"
 #include "TalonSRX.h"
+#include "Threads.h"
 #include "Timer.h"
 #include "Ultrasonic.h"
 #include "Utility.h"

wpilibc/athena/src/Threads.cpp

@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/Threads.h"
+#include "Threads.h"
+
+#include "ErrorBase.h"
+#include "HAL/HAL.h"
+
+namespace frc {
+/**
+ * Get the thread priority for the specified thread.
+ *
+ * @param thread Reference to the thread to get the priority for
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @return The current thread priority. Scaled 1-99, with 1 being highest.
+ */
+int GetThreadPriority(std::thread& thread, bool* isRealTime) {
+  int32_t status = 0;
+  HAL_Bool rt = false;
+  auto native = thread.native_handle();
+  auto ret = HAL_GetThreadPriority(&native, &rt, &status);
+  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  *isRealTime = rt;
+  return ret;
+}
+
+/**
+ * Get the thread priority for the current thread
+ *
+ * @param isRealTime Set to true if thread is realtime, otherwise false
+ * @return The current thread priority. Scaled 1-99.
+ */
+int GetCurrentThreadPriority(bool* isRealTime) {
+  int32_t status = 0;
+  HAL_Bool rt = false;
+  auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
+  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  *isRealTime = rt;
+  return ret;
+}
+
+/**
+ * Sets the thread priority for the specified thread
+ *
+ * @param thread Reference to the thread to set the priority of
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest. On RoboRIO, priority is ignored for non realtime setting
+ *
+ * @return The success state of setting the priority
+ */
+bool SetThreadPriority(std::thread& thread, bool realTime, int priority) {
+  int32_t status = 0;
+  auto native = thread.native_handle();
+  auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
+  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  return ret;
+}
+
+/**
+ * Sets the thread priority for the current thread
+ *
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest. On RoboRIO, priority is ignored for non realtime setting
+ *
+ * @return The success state of setting the priority
+ */
+bool SetCurrentThreadPriority(bool realTime, int priority) {
+  int32_t status = 0;
+  auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
+  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  return ret;
+}
+}  // namespace frc

wpilibj/athena.gradle

@@ -171,6 +171,7 @@ task jniHeaders {
             args 'edu.wpi.first.wpilibj.hal.PDPJNI'
             args 'edu.wpi.first.wpilibj.hal.PowerJNI'
             args 'edu.wpi.first.wpilibj.hal.SerialPortJNI'
+            args 'edu.wpi.first.wpilibj.hal.ThreadsJNI'
         }
     }
 }

wpilibj/src/athena/cpp/lib/ThreadsJNI.cpp

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <assert.h>
+#include <jni.h>
+#include "HAL/cpp/Log.h"
+
+#include "edu_wpi_first_wpilibj_hal_ThreadsJNI.h"
+
+#include "HAL/Threads.h"
+#include "HALUtil.h"
+
+using namespace frc;
+
+// set the logging level
+TLogLevel threadsJNILogLevel = logWARNING;
+
+#define THREADSJNI_LOG(level)     \
+  if (level > threadsJNILogLevel) \
+    ;                            \
+  else                           \
+  Log().Get(level)
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_ThreadsJNI
+ * Method:    GetCurrentThreadPriority
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_ThreadsJNI_getCurrentThreadPriority
+  (JNIEnv *env, jclass) {
+  THREADSJNI_LOG(logDEBUG) << "Callling GetCurrentThreadPriority";
+  int32_t status = 0;
+  HAL_Bool isRT = false;
+  auto ret = HAL_GetCurrentThreadPriority(&isRT, &status);
+  CheckStatus(env, status);
+  return (jint)ret;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_ThreadsJNI
+ * Method:    GetCurrentThreadIsRealTime
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_ThreadsJNI_getCurrentThreadIsRealTime
+  (JNIEnv *env, jclass) {
+  THREADSJNI_LOG(logDEBUG) << "Callling GetCurrentThreadIsRealTime";
+  int32_t status = 0;
+  HAL_Bool isRT = false;
+  HAL_GetCurrentThreadPriority(&isRT, &status);
+  CheckStatus(env, status);
+  return (jboolean)isRT;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_ThreadsJNI
+ * Method:    SetCurrentThreadPriority
+ * Signature: (ZI)Z
+ */
+JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_ThreadsJNI_setCurrentThreadPriority
+  (JNIEnv *env, jclass, jboolean realTime, jint priority) {
+  THREADSJNI_LOG(logDEBUG) << "Callling SetCurrentThreadPriority";
+  int32_t status = 0;
+  auto ret = HAL_SetCurrentThreadPriority((HAL_Bool)realTime, (int32_t)priority, &status);
+  CheckStatus(env, status);
+  return (jboolean)ret;
+}
+
+}

wpilibj/src/athena/java/edu/wpi/first/wpilibj/Threads.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2016. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.hal.ThreadsJNI;
+
+public class Threads {
+
+  /**
+  * Get the thread priority for the current thread.
+  * @return The current thread priority. Scaled 1-99.
+  */
+  public static int getCurrentThreadPriority() {
+    return ThreadsJNI.getCurrentThreadPriority();
+  }
+
+  /**
+  * Get if the current thread is realtime.
+  * @return If the current thread is realtime
+  */
+  public static boolean getCurrentThreadIsRealTime() {
+    return ThreadsJNI.getCurrentThreadIsRealTime();
+  }
+
+  /**
+  * Sets the thread priority for the current thread.
+  *
+  * @param realTime Set to true to set a realtime priority, false for standard
+  *     priority
+  * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+  *     highest. On RoboRIO, priority is ignored for non realtime setting
+  *
+  * @return The success state of setting the priority
+  */
+  public static boolean setCurrentThreadPriority(boolean realTime, int priority) {
+    return ThreadsJNI.setCurrentThreadPriority(realTime, priority);
+  }
+}

wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/ThreadsJNI.java

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+public class ThreadsJNI extends JNIWrapper {
+  public static native int getCurrentThreadPriority();
+
+  public static native boolean getCurrentThreadIsRealTime();
+
+  public static native boolean setCurrentThreadPriority(boolean realTime, int priority);
+}