[hal] Convert DutyCycle Raw output to be a high time measurement (#4466)

The existing raw time has an issue where it jumps around, as in the FPGA if the frequency is not a multiple or divisor of 25 Mhz it jumps around by 1 every second. While waiting on an FPGA change, update the API to make raw output give nanoseconds rather then a scaled value. This does a longer read cycle to get the correct value, but in the future if a fast FPGA function is added this can be easily changed.

hal/src/main/native/athena/AnalogTrigger.cpp

@@ -5,6 +5,7 @@
 #include "hal/AnalogTrigger.h"
 
 #include "AnalogInternal.h"
+#include "ConstantsInternal.h"
 #include "DutyCycleInternal.h"
 #include "HALInitializer.h"
 #include "HALInternal.h"
@@ -147,16 +148,10 @@ void HAL_SetAnalogTriggerLimitsDutyCycle(
     return;
   }
 
-  int32_t scaleFactor =
-      HAL_GetDutyCycleOutputScaleFactor(trigger->handle, status);
-  if (*status != 0) {
-    return;
-  }
-
-  trigger->trigger->writeLowerLimit(static_cast<int32_t>(scaleFactor * lower),
-                                    status);
-  trigger->trigger->writeUpperLimit(static_cast<int32_t>(scaleFactor * upper),
-                                    status);
+  trigger->trigger->writeLowerLimit(
+      static_cast<int32_t>(kDutyCycleScaleFactor * lower), status);
+  trigger->trigger->writeUpperLimit(
+      static_cast<int32_t>(kDutyCycleScaleFactor * upper), status);
 }
 
 void HAL_SetAnalogTriggerLimitsVoltage(

hal/src/main/native/athena/ConstantsInternal.h

@@ -9,5 +9,6 @@
 namespace hal {
 
 constexpr int32_t kSystemClockTicksPerMicrosecond = 40;
+constexpr int32_t kDutyCycleScaleFactor = 4e7 - 1;
 
 }  // namespace hal

hal/src/main/native/athena/DutyCycle.cpp

@@ -6,6 +6,7 @@
 
 #include <memory>
 
+#include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "DutyCycleInternal.h"
 #include "HALInitializer.h"
@@ -30,8 +31,6 @@ void InitializeDutyCycle() {
 }  // namespace init
 }  // namespace hal
 
-static constexpr int32_t kScaleFactor = 4e7 - 1;
-
 extern "C" {
 HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
                                             HAL_AnalogTriggerType triggerType,
@@ -90,12 +89,6 @@ int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
 
 double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
                               int32_t* status) {
-  return HAL_GetDutyCycleOutputRaw(dutyCycleHandle, status) /
-         static_cast<double>(kScaleFactor);
-}
-
-int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
-                                  int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
   if (!dutyCycle) {
     *status = HAL_HANDLE_ERROR;
@@ -104,12 +97,43 @@ int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
 
   // TODO Handle Overflow
   unsigned char overflow = 0;
-  return dutyCycle->dutyCycle->readOutput(&overflow, status);
+  uint32_t output = dutyCycle->dutyCycle->readOutput(&overflow, status);
+  return output / static_cast<double>(kDutyCycleScaleFactor);
+}
+
+int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
+                                 int32_t* status) {
+  auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+  if (!dutyCycle) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  // TODO Handle Overflow
+  unsigned char overflow = 0;
+  uint32_t freq0 = dutyCycle->dutyCycle->readFrequency(&overflow, status);
+  uint32_t output = dutyCycle->dutyCycle->readOutput(&overflow, status);
+  uint32_t freq1 = dutyCycle->dutyCycle->readFrequency(&overflow, status);
+  if (*status != 0) {
+    return 0;
+  }
+  if (freq0 != freq1) {
+    // Frequency rolled over. Reread output
+    output = dutyCycle->dutyCycle->readOutput(&overflow, status);
+    if (*status != 0) {
+      return 0;
+    }
+  }
+  if (freq1 == 0) {
+    return 0;
+  }
+  // Output will be at max 4e7, so x25 will still fit in a 32 bit signed int.
+  return (output / freq1) * 25;
 }
 
 int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
                                           int32_t* status) {
-  return kScaleFactor;
+  return kDutyCycleScaleFactor;
 }
 
 int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,

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

@@ -74,15 +74,15 @@ Java_edu_wpi_first_hal_DutyCycleJNI_getOutput
 
 /*
  * Class:     edu_wpi_first_hal_DutyCycleJNI
- * Method:    getOutputRaw
+ * Method:    getHighTime
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_DutyCycleJNI_getOutputRaw
+Java_edu_wpi_first_hal_DutyCycleJNI_getHighTime
   (JNIEnv* env, jclass, jint handle)
 {
   int32_t status = 0;
-  auto retVal = HAL_GetDutyCycleOutputRaw(
+  auto retVal = HAL_GetDutyCycleHighTime(
       static_cast<HAL_DutyCycleHandle>(handle), &status);
   CheckStatus(env, status);
   return retVal;

hal/src/main/native/include/hal/DutyCycle.h

@@ -71,24 +71,21 @@ double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
                               int32_t* status);
 
 /**
- * Get the raw output ratio of the duty cycle signal.
- *
- * <p> 0 means always low, an output equal to
- * GetOutputScaleFactor() means always high.
+ * Get the raw high time of the duty cycle signal.
  *
  * @param[in] dutyCycleHandle the duty cycle handle
  * @param[out] status Error status variable. 0 on success.
- * @return output ratio in raw units
+ * @return high time of last pulse in nanoseconds
  */
-int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
-                                  int32_t* status);
+int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
+                                 int32_t* status);
 
 /**
  * Get the scale factor of the output.
  *
  * <p> An output equal to this value is always high, and then linearly scales
- * down to 0. Divide the result of getOutputRaw by this in order to get the
- * percentage between 0 and 1.
+ * down to 0. Divide a raw result by this in order to get the
+ * percentage between 0 and 1. Used by DMA.
  *
  * @param[in] dutyCycleHandle the duty cycle handle
  * @param[out] status Error status variable. 0 on success.

hal/src/main/native/sim/DutyCycle.cpp

@@ -84,6 +84,7 @@ int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
   }
   return SimDutyCycleData[dutyCycle->index].frequency;
 }
+
 double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
                               int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
@@ -93,20 +94,29 @@ double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
   }
   return SimDutyCycleData[dutyCycle->index].output;
 }
-int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
-                                  int32_t* status) {
+
+int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
+                                 int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
   if (dutyCycle == nullptr) {
     *status = HAL_HANDLE_ERROR;
     return 0;
   }
-  return SimDutyCycleData[dutyCycle->index].output *
-         HAL_GetDutyCycleOutputScaleFactor(dutyCycleHandle, status);
+
+  if (SimDutyCycleData[dutyCycle->index].frequency == 0) {
+    return 0;
+  }
+
+  double periodSeconds = 1.0 / SimDutyCycleData[dutyCycle->index].frequency;
+  double periodNanoSeconds = periodSeconds * 1e9;
+  return periodNanoSeconds * SimDutyCycleData[dutyCycle->index].output;
 }
+
 int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
                                           int32_t* status) {
   return 4e7 - 1;
 }
+
 int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
                                   int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);