[hal] Add frequency support to DutyCycle (#8076)

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

@@ -88,16 +88,47 @@ void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
 void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
                                HAL_SimDeviceHandle device) {}
 
-int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
-                                  int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+double HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+                                 int32_t* status) {
+  auto port = smartIoHandles->Get(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  uint16_t ret = 0;
+  *status = port->GetPwmInputPeriodMicroseconds(&ret);
+
+  if (ret == 0) {
+    return 0.0;
+  }
+
+  return 1000000.0 / ret;
 }
 
 double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
                               int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0.0;
+  }
+
+  uint16_t highTime = 0;
+  *status = port->GetPwmInputMicroseconds(&highTime);
+
+  uint16_t period = 0;
+  *status = port->GetPwmInputPeriodMicroseconds(&period);
+
+  if (period == 0) {
+    return 0.0;
+  }
+
+  if (highTime >= period) {
+    return 1.0;
+  }
+
+  return static_cast<double>(highTime) / static_cast<double>(period);
 }
 
 int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
@@ -105,23 +136,11 @@ int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
   auto port = smartIoHandles->Get(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
   if (port == nullptr) {
     *status = HAL_HANDLE_ERROR;
-    return false;
+    return 0;
   }
 
   uint16_t ret = 0;
   *status = port->GetPwmInputMicroseconds(&ret);
   return static_cast<int32_t>(ret) * 1000;
 }
-
-int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
-                                          int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
-}
-
-int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
-                                  int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
-}
 }  // extern "C"

hal/src/main/native/systemcore/SmartIo.cpp

@@ -35,11 +35,11 @@ int32_t SmartIo::InitializeMode(SmartIoMode mode) {
   modePublisher = inst.GetIntegerTopic(subTableString + "type").Publish();
   getSubscriber =
       inst.GetIntegerTopic(subTableString + "valget").Subscribe(0, options);
-  frequencySubscriber =
-      inst.GetIntegerTopic(subTableString + "freqget").Subscribe(0, options);
+  periodGetSubscriber =
+      inst.GetIntegerTopic(subTableString + "periodget").Subscribe(0, options);
   setPublisher =
       inst.GetIntegerTopic(subTableString + "valset").Publish(options);
-  periodPublisher =
+  periodSetPublisher =
       inst.GetIntegerTopic(subTableString + "periodset").Publish(options);
 
   currentMode = mode;
@@ -107,6 +107,17 @@ int32_t SmartIo::GetPwmInputMicroseconds(uint16_t* microseconds) {
   return 0;
 }
 
+int32_t SmartIo::GetPwmInputPeriodMicroseconds(uint16_t* microseconds) {
+  if (currentMode != SmartIoMode::PwmInput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  int val = periodGetSubscriber.Get();
+  *microseconds = val;
+
+  return 0;
+}
+
 int32_t SmartIo::SetPwmOutputPeriod(PwmOutputPeriod period) {
   if (currentMode != SmartIoMode::PwmOutput) {
     return INCOMPATIBLE_STATE;
@@ -117,7 +128,7 @@ int32_t SmartIo::SetPwmOutputPeriod(PwmOutputPeriod period) {
     case PwmOutputPeriod::k10ms:
     case PwmOutputPeriod::k5ms:
     case PwmOutputPeriod::k2ms:
-      periodPublisher.Set(static_cast<int>(period));
+      periodSetPublisher.Set(static_cast<int>(period));
       return 0;
 
     default:

hal/src/main/native/systemcore/SmartIo.h

@@ -19,12 +19,12 @@ constexpr int32_t kPwmAlwaysHigh = 0xFFFF;
 
 enum class SmartIoMode {
   DigitalInput = 0,
-  DigitalOutput,
-  AnalogInput,
-  PwmInput,
-  PwmOutput,
-  SingleCounterRising,
-  SingleCounterFalling,
+  DigitalOutput = 1,
+  AnalogInput = 2,
+  PwmInput = 3,
+  PwmOutput = 4,
+  SingleCounterRising = 5,
+  SingleCounterFalling = 6,
 };
 
 enum class PwmOutputPeriod {
@@ -43,8 +43,8 @@ struct SmartIo {
   nt::IntegerPublisher setPublisher;
   nt::IntegerSubscriber getSubscriber;
 
-  nt::IntegerPublisher periodPublisher;
-  nt::IntegerSubscriber frequencySubscriber;
+  nt::IntegerPublisher periodSetPublisher;
+  nt::IntegerSubscriber periodGetSubscriber;
 
   int32_t InitializeMode(SmartIoMode mode);
   int32_t SwitchDioDirection(bool input);
@@ -53,6 +53,7 @@ struct SmartIo {
   int32_t GetDigitalInput(bool* value);
 
   int32_t GetPwmInputMicroseconds(uint16_t* microseconds);
+  int32_t GetPwmInputPeriodMicroseconds(uint16_t* microseconds);
 
   int32_t SetPwmOutputPeriod(PwmOutputPeriod period);
 

hal/src/main/native/systemcore/mockdata/DutyCycleData.cpp

@@ -7,16 +7,9 @@
 #include "hal/simulation/SimDataValue.h"
 
 extern "C" {
-int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) {
-  return 0;
-}
 
 void HALSIM_ResetDutyCycleData(int32_t index) {}
 
-int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) {
-  return 0;
-}
-
 HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
   return 0;
 }
@@ -25,7 +18,7 @@ HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
   HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, RETURN)
 
 DEFINE_CAPI(HAL_Bool, Initialized, false)
-DEFINE_CAPI(int32_t, Frequency, 0)
+DEFINE_CAPI(double, Frequency, 0)
 DEFINE_CAPI(double, Output, 0)
 
 void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,