[hal, wpilib] New DS thread model and implementation (#3787)

The current DS thread model has some pretty major issues. It makes it difficult to know if all data is from the same remote packet, and if the data changes while the robot loop is running. Additionally, the DS thread is used for a few other things (MotorSafety and State Tracking for EducationalRobot). This also makes sim difficult, as user code has to wait for the thread to know it has new data.

This change completely rethinks how threading works in the driver station model.

First, the DS HAL system receives a new data callback, either from Netcomm or DriverStationSim. Inside the context of this callback, all the low latency data is read and put into a cache. Doing some investigation on the robot side, this is perfectly safe to do, and also ensures a ds packet will not be parsed before we finish reading the current packet data.

After all data is read, the cache is swapped with a 2nd buffer. This buffer just stores the data, none of the HAL DS calls read from this buffer. An event is then fired, stating there is new data ready to go.

Robot code calls HAL_UpdateDSData(). This swaps the 2nd buffer with a 3rd buffer, which always contains the current data. This data will not be updated until HAL_UpdateDSData is called again. Which solves the state problem.

The high level driver station classes have. an updateData() call, which calls HAL_UpdateDSData, and then update button state variables, then data log and update the NT FMS data table (Java also caches across the JNI boundary here, but that could trivially be removed). An extra event provider is provided, allowing other threads to know when this call has been completed.

IterativeRobotBase calls DS.updateData() at the beginning of each loop, and only once per loop. This means all commands will always have the same state.

All of this means there is no longer a DS thread. Everything happens synchronously. This means Sim and testing is easier, as you can just call DriverStationSim.NotifyNewData(), and then DriverStation.UpdateData(), and you can guarantee that all the DriverStation.*** data is up to date.

As for Motor Safety and Educational Robot State Handling, those can all be handled by their own threads. The Educational Thread only needs to run under EducationalRobot, and MotorSafety will only be started if there is a motor safety object enabled.

hal/src/main/native/cpp/jni/HAL.cpp

@@ -106,310 +106,6 @@ Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative
   HAL_SimPeriodicAfter();
 }
 
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    observeUserProgramStarting
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_observeUserProgramStarting
-  (JNIEnv*, jclass)
-{
-  HAL_ObserveUserProgramStarting();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    observeUserProgramDisabled
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_observeUserProgramDisabled
-  (JNIEnv*, jclass)
-{
-  HAL_ObserveUserProgramDisabled();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    observeUserProgramAutonomous
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_observeUserProgramAutonomous
-  (JNIEnv*, jclass)
-{
-  HAL_ObserveUserProgramAutonomous();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    observeUserProgramTeleop
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_observeUserProgramTeleop
-  (JNIEnv*, jclass)
-{
-  HAL_ObserveUserProgramTeleop();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    observeUserProgramTest
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_observeUserProgramTest
-  (JNIEnv*, jclass)
-{
-  HAL_ObserveUserProgramTest();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    report
- * Signature: (IIILjava/lang/String;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_report
-  (JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber,
-   jint paramContext, jstring paramFeature)
-{
-  JStringRef featureStr{paramEnv, paramFeature};
-  jint returnValue = HAL_Report(paramResource, paramInstanceNumber,
-                                paramContext, featureStr.c_str());
-  return returnValue;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    nativeGetControlWord
- * Signature: ()I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_nativeGetControlWord
-  (JNIEnv*, jclass)
-{
-  static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
-                "Java int must match the size of control word");
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  jint retVal = 0;
-  std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
-  return retVal;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    nativeGetAllianceStation
- * Signature: ()I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_nativeGetAllianceStation
-  (JNIEnv*, jclass)
-{
-  int32_t status = 0;
-  auto allianceStation = HAL_GetAllianceStation(&status);
-  return static_cast<jint>(allianceStation);
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickAxes
- * Signature: (B[F)S
- */
-JNIEXPORT jshort JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickAxes
-  (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
-{
-  HAL_JoystickAxes axes;
-  HAL_GetJoystickAxes(joystickNum, &axes);
-
-  jsize javaSize = env->GetArrayLength(axesArray);
-  if (axes.count > javaSize) {
-    ThrowIllegalArgumentException(
-        env,
-        fmt::format("Native array size larger then passed in java array "
-                    "size\nNative Size: {} Java Size: {}",
-                    static_cast<int>(axes.count), static_cast<int>(javaSize)));
-    return 0;
-  }
-
-  env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes);
-
-  return axes.count;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickPOVs
- * Signature: (B[S)S
- */
-JNIEXPORT jshort JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickPOVs
-  (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
-{
-  HAL_JoystickPOVs povs;
-  HAL_GetJoystickPOVs(joystickNum, &povs);
-
-  jsize javaSize = env->GetArrayLength(povsArray);
-  if (povs.count > javaSize) {
-    ThrowIllegalArgumentException(
-        env,
-        fmt::format("Native array size larger then passed in java array "
-                    "size\nNative Size: {} Java Size: {}",
-                    static_cast<int>(povs.count), static_cast<int>(javaSize)));
-    return 0;
-  }
-
-  env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs);
-
-  return povs.count;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickButtons
- * Signature: (BLjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickButtons
-  (JNIEnv* env, jclass, jbyte joystickNum, jobject count)
-{
-  HAL_JoystickButtons joystickButtons;
-  HAL_GetJoystickButtons(joystickNum, &joystickButtons);
-  jbyte* countPtr =
-      reinterpret_cast<jbyte*>(env->GetDirectBufferAddress(count));
-  *countPtr = joystickButtons.count;
-  return joystickButtons.buttons;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    setJoystickOutputs
- * Signature: (BISS)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_setJoystickOutputs
-  (JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble,
-   jshort rightRumble)
-{
-  return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble);
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickIsXbox
- * Signature: (B)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickIsXbox
-  (JNIEnv*, jclass, jbyte port)
-{
-  return HAL_GetJoystickIsXbox(port);
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickType
- * Signature: (B)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickType
-  (JNIEnv*, jclass, jbyte port)
-{
-  return HAL_GetJoystickType(port);
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickName
- * Signature: (B)Ljava/lang/String;
- */
-JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickName
-  (JNIEnv* env, jclass, jbyte port)
-{
-  char* joystickName = HAL_GetJoystickName(port);
-  jstring str = MakeJString(env, joystickName);
-  HAL_FreeJoystickName(joystickName);
-  return str;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getJoystickAxisType
- * Signature: (BB)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_getJoystickAxisType
-  (JNIEnv*, jclass, jbyte joystickNum, jbyte axis)
-{
-  return HAL_GetJoystickAxisType(joystickNum, axis);
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    isNewControlData
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_HAL_isNewControlData
-  (JNIEnv*, jclass)
-{
-  return static_cast<jboolean>(HAL_IsNewControlData());
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    waitForDSData
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_waitForDSData
-  (JNIEnv* env, jclass)
-{
-  HAL_WaitForDSData();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    releaseDSMutex
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_HAL_releaseDSMutex
-  (JNIEnv* env, jclass)
-{
-  HAL_ReleaseDSMutex();
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    waitForDSDataTimeout
- * Signature: (D)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_HAL_waitForDSDataTimeout
-  (JNIEnv*, jclass, jdouble timeout)
-{
-  return static_cast<jboolean>(HAL_WaitForDSDataTimeout(timeout));
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getMatchTime
- * Signature: ()D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_HAL_getMatchTime
-  (JNIEnv* env, jclass)
-{
-  int32_t status = 0;
-  return HAL_GetMatchTime(&status);
-}
-
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    getSystemActive
@@ -440,58 +136,6 @@ Java_edu_wpi_first_hal_HAL_getBrownedOut
   return val;
 }
 
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    getMatchInfo
- * Signature: (Ljava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_getMatchInfo
-  (JNIEnv* env, jclass, jobject info)
-{
-  HAL_MatchInfo matchInfo;
-  auto status = HAL_GetMatchInfo(&matchInfo);
-  if (status == 0) {
-    SetMatchInfoObject(env, info, matchInfo);
-  }
-  return status;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    sendError
- * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_sendError
-  (JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
-   jstring details, jstring location, jstring callStack, jboolean printMsg)
-{
-  JStringRef detailsStr{env, details};
-  JStringRef locationStr{env, location};
-  JStringRef callStackStr{env, callStack};
-
-  jint returnValue =
-      HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
-                    locationStr.c_str(), callStackStr.c_str(), printMsg);
-  return returnValue;
-}
-
-/*
- * Class:     edu_wpi_first_hal_HAL
- * Method:    sendConsoleLine
- * Signature: (Ljava/lang/String;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_HAL_sendConsoleLine
-  (JNIEnv* env, jclass, jstring line)
-{
-  JStringRef lineStr{env, line};
-
-  jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
-  return returnValue;
-}
-
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    getPortWithModule