Add FPGA Duty Cycle support (#1987)

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. 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.                                                               */
@@ -8,9 +8,11 @@
 #include "hal/AnalogTrigger.h"
 
 #include "AnalogInternal.h"
+#include "DutyCycleInternal.h"
 #include "HALInitializer.h"
 #include "PortsInternal.h"
 #include "hal/AnalogInput.h"
+#include "hal/DutyCycle.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
@@ -21,7 +23,7 @@ namespace {
 
 struct AnalogTrigger {
   std::unique_ptr<tAnalogTrigger> trigger;
-  HAL_AnalogInputHandle analogHandle;
+  HAL_Handle handle;
   uint8_t index;
 };
 
@@ -46,7 +48,7 @@ void InitializeAnalogTrigger() {
 extern "C" {
 
 HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
-    HAL_AnalogInputHandle portHandle, int32_t* index, int32_t* status) {
+    HAL_AnalogInputHandle portHandle, int32_t* status) {
   hal::init::CheckInit();
   // ensure we are given a valid and active AnalogInput handle
   auto analog_port = analogInputHandles->Get(portHandle);
@@ -64,19 +66,46 @@ HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
     *status = HAL_HANDLE_ERROR;
     return HAL_kInvalidHandle;
   }
-  trigger->analogHandle = portHandle;
+  trigger->handle = portHandle;
   trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
-  *index = trigger->index;
 
   trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
   trigger->trigger->writeSourceSelect_Channel(analog_port->channel, status);
   return handle;
 }
 
+HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
+    HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
+  hal::init::CheckInit();
+  // ensure we are given a valid and active DutyCycle handle
+  auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+  if (dutyCycle == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return HAL_kInvalidHandle;
+  }
+  HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
+  if (handle == HAL_kInvalidHandle) {
+    *status = NO_AVAILABLE_RESOURCES;
+    return HAL_kInvalidHandle;
+  }
+  auto trigger = analogTriggerHandles->Get(handle);
+  if (trigger == nullptr) {  // would only occur on thread issue
+    *status = HAL_HANDLE_ERROR;
+    return HAL_kInvalidHandle;
+  }
+  trigger->handle = dutyCycleHandle;
+  trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
+
+  trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
+  trigger->trigger->writeSourceSelect_Channel(dutyCycle->index, status);
+  trigger->trigger->writeSourceSelect_DutyCycle(true, status);
+  return handle;
+}
+
 void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
                             int32_t* status) {
   analogTriggerHandles->Free(analogTriggerHandle);
-  // caller owns the analog input handle.
+  // caller owns the input handle.
 }
 
 void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
@@ -89,11 +118,46 @@ void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
   }
   if (lower > upper) {
     *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+    return;
   }
   trigger->trigger->writeLowerLimit(lower, status);
   trigger->trigger->writeUpperLimit(upper, status);
 }
 
+void HAL_SetAnalogTriggerLimitsDutyCycle(
+    HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
+    int32_t* status) {
+  auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+  if (trigger == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  if (getHandleType(trigger->handle) != HAL_HandleEnum::DutyCycle) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  if (lower > upper) {
+    *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+    return;
+  }
+
+  if (lower < 0.0 || upper > 1.0) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    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->writeLowerLimit(static_cast<int32_t>(scaleFactor * upper),
+                                    status);
+}
+
 void HAL_SetAnalogTriggerLimitsVoltage(
     HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
     int32_t* status) {
@@ -102,16 +166,22 @@ void HAL_SetAnalogTriggerLimitsVoltage(
     *status = HAL_HANDLE_ERROR;
     return;
   }
+
+  if (getHandleType(trigger->handle) != HAL_HandleEnum::AnalogInput) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
   if (lower > upper) {
     *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
+    return;
   }
 
   // TODO: This depends on the averaged setting.  Only raw values will work as
   // is.
   trigger->trigger->writeLowerLimit(
-      HAL_GetAnalogVoltsToValue(trigger->analogHandle, lower, status), status);
+      HAL_GetAnalogVoltsToValue(trigger->handle, lower, status), status);
   trigger->trigger->writeUpperLimit(
-      HAL_GetAnalogVoltsToValue(trigger->analogHandle, upper, status), status);
+      HAL_GetAnalogVoltsToValue(trigger->handle, upper, status), status);
 }
 
 void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
@@ -121,7 +191,8 @@ void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  if (trigger->trigger->readSourceSelect_Filter(status) != 0) {
+  if (trigger->trigger->readSourceSelect_Filter(status) != 0 ||
+      trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
     *status = INCOMPATIBLE_STATE;
     // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not
     // support average and filtering at the same time.");
@@ -136,7 +207,8 @@ void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  if (trigger->trigger->readSourceSelect_Averaged(status) != 0) {
+  if (trigger->trigger->readSourceSelect_Averaged(status) != 0 ||
+      trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
     *status = INCOMPATIBLE_STATE;
     // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not "
     // "support average and filtering at the same time.");
@@ -177,16 +249,28 @@ HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
     case HAL_Trigger_kInWindow:
       result =
           trigger->trigger->readOutput_InHysteresis(trigger->index, status);
-      break;  // XXX: Backport
+      break;
     case HAL_Trigger_kState:
       result = trigger->trigger->readOutput_OverLimit(trigger->index, status);
-      break;  // XXX: Backport
+      break;
     case HAL_Trigger_kRisingPulse:
+      result = trigger->trigger->readOutput_Rising(trigger->index, status);
+      break;
     case HAL_Trigger_kFallingPulse:
-      *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
-      return false;
+      result = trigger->trigger->readOutput_Falling(trigger->index, status);
+      break;
   }
   return result;
 }
 
+int32_t HAL_GetAnalogTriggerFPGAIndex(
+    HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
+  auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
+  if (trigger == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return -1;
+  }
+  return trigger->index;
+}
+
 }  // extern "C"

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

@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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/DutyCycle.h"
+
+#include <memory>
+
+#include "DigitalInternal.h"
+#include "DutyCycleInternal.h"
+#include "HALInitializer.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+namespace hal {
+LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+                      HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+namespace init {
+void InitializeDutyCycle() {
+  static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+                               HAL_HandleEnum::DutyCycle>
+      dcH;
+  dutyCycleHandles = &dcH;
+}
+}  // namespace init
+}  // namespace hal
+
+static constexpr int32_t kScaleFactor = 4e7 - 1;
+
+extern "C" {
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
+                                            HAL_AnalogTriggerType triggerType,
+                                            int32_t* status) {
+  hal::init::CheckInit();
+
+  bool routingAnalogTrigger = false;
+  uint8_t routingChannel = 0;
+  uint8_t routingModule = 0;
+  bool success =
+      remapDigitalSource(digitalSourceHandle, triggerType, routingChannel,
+                         routingModule, routingAnalogTrigger);
+
+  if (!success) {
+    *status = HAL_HANDLE_ERROR;
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
+  if (handle == HAL_kInvalidHandle) {
+    *status = NO_AVAILABLE_RESOURCES;
+    return HAL_kInvalidHandle;
+  }
+
+  auto dutyCycle = dutyCycleHandles->Get(handle);
+  uint32_t index = static_cast<uint32_t>(getHandleIndex(handle));
+  dutyCycle->dutyCycle.reset(tDutyCycle::create(index, status));
+
+  dutyCycle->dutyCycle->writeSource_AnalogTrigger(routingAnalogTrigger, status);
+  dutyCycle->dutyCycle->writeSource_Channel(routingChannel, status);
+  dutyCycle->dutyCycle->writeSource_Module(routingModule, status);
+  dutyCycle->index = index;
+
+  return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+  // Just free it, the unique ptr will take care of everything else
+  dutyCycleHandles->Free(dutyCycleHandle);
+}
+
+int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
+                                  int32_t* status) {
+  auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+  if (!dutyCycle) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return dutyCycle->dutyCycle->readFrequency(status);
+}
+
+double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
+                              int32_t* status) {
+  return HAL_GetDutyCycleOutputRaw(dutyCycleHandle, status) / kScaleFactor;
+}
+
+int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
+                                  int32_t* status) {
+  auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+  if (!dutyCycle) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return dutyCycle->dutyCycle->readOutput(status);
+}
+
+int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
+                                          int32_t* status) {
+  return kScaleFactor;
+}
+
+int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
+                                  int32_t* status) {
+  auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
+  if (!dutyCycle) {
+    *status = HAL_HANDLE_ERROR;
+    return -1;
+  }
+  return dutyCycle->index;
+}
+}  // extern "C"

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

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. 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 <memory>
+
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+namespace hal {
+struct DutyCycle {
+  std::unique_ptr<tDutyCycle> dutyCycle;
+  int index;
+};
+
+extern LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
+                             HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
+}  // namespace hal

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

@@ -56,6 +56,7 @@ void InitializeHAL() {
   InitializeCounter();
   InitializeDigitalInternal();
   InitializeDIO();
+  InitializeDutyCycle();
   InitializeEncoder();
   InitializeFPGAEncoder();
   InitializeFRCDriverStation();

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

@@ -32,6 +32,7 @@ extern void InitializeConstants();
 extern void InitializeCounter();
 extern void InitializeDigitalInternal();
 extern void InitializeDIO();
+extern void InitializeDutyCycle();
 extern void InitializeEncoder();
 extern void InitializeFPGAEncoder();
 extern void InitializeFRCDriverStation();

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. 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.                                                               */
@@ -37,5 +37,6 @@ int32_t HAL_GetNumPCMModules(void) { return kNumPCMModules; }
 int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
 int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
 int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
+int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
 
 }  // extern "C"

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. 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.                                                               */
@@ -35,5 +35,6 @@ constexpr int32_t kNumPCMModules = 63;
 constexpr int32_t kNumSolenoidChannels = 8;
 constexpr int32_t kNumPDPModules = 63;
 constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumDutyCycles = 8;
 
 }  // namespace hal