[hal] Add initial PWM support for systemcore (#7525)

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

@@ -15,12 +15,53 @@
 #include "HALInitializer.h"
 #include "HALInternal.h"
 #include "PortsInternal.h"
+#include "SmartIo.h"
 #include "hal/Errors.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
 
 using namespace hal;
 
+static inline int32_t GetMaxPositivePwm(SmartIo* port) {
+  return port->maxPwm;
+}
+
+static inline int32_t GetMinPositivePwm(SmartIo* port) {
+  if (port->eliminateDeadband) {
+    return port->deadbandMaxPwm;
+  } else {
+    return port->centerPwm + 1;
+  }
+}
+
+static inline int32_t GetCenterPwm(SmartIo* port) {
+  return port->centerPwm;
+}
+
+static inline int32_t GetMaxNegativePwm(SmartIo* port) {
+  if (port->eliminateDeadband) {
+    return port->deadbandMinPwm;
+  } else {
+    return port->centerPwm - 1;
+  }
+}
+
+static inline int32_t GetMinNegativePwm(SmartIo* port) {
+  return port->minPwm;
+}
+
+static inline int32_t GetPositiveScaleFactor(SmartIo* port) {
+  return GetMaxPositivePwm(port) - GetMinPositivePwm(port);
+}  ///< The scale for positive speeds.
+
+static inline int32_t GetNegativeScaleFactor(SmartIo* port) {
+  return GetMaxNegativePwm(port) - GetMinNegativePwm(port);
+}  ///< The scale for negative speeds.
+
+static inline int32_t GetFullRangeScaleFactor(SmartIo* port) {
+  return GetMaxPositivePwm(port) - GetMinNegativePwm(port);
+}  ///< The scale for positions.
+
 namespace hal::init {
 void InitializePWM() {}
 }  // namespace hal::init
@@ -31,108 +72,328 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
                                         const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
-  *status = HAL_HANDLE_ERROR;
-  return HAL_kInvalidHandle;
+
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumSmartIo) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                     kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port =
+      smartIoHandles->Allocate(channel, HAL_HandleEnum::PWM, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                       kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(SmartIoMode::PWMOutput);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::PWM);
+    return HAL_kInvalidHandle;
+  }
+
+  // Defaults to allow an always valid config.
+  HAL_SetPWMConfigMicroseconds(handle, 2000, 1501, 1500, 1499, 1000, status);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::PWM);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
 }
 
-void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle) {}
+void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle) {
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    return;
+  }
+
+  smartIoHandles->Free(pwmPortHandle, HAL_HandleEnum::PWM);
+
+  // Wait for no other object to hold this handle.
+  auto start = hal::fpga_clock::now();
+  while (port.use_count() != 1) {
+    auto current = hal::fpga_clock::now();
+    if (start + std::chrono::seconds(1) < current) {
+      std::puts("PWM handle free timeout");
+      std::fflush(stdout);
+      break;
+    }
+    std::this_thread::yield();
+  }
+}
 
 HAL_Bool HAL_CheckPWMChannel(int32_t channel) {
-  return false;
+  return channel < kNumSmartIo && channel >= 0;
 }
 
 void HAL_SetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle, int32_t max,
                                   int32_t deadbandMax, int32_t center,
                                   int32_t deadbandMin, int32_t min,
                                   int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  port->maxPwm = max;
+  port->deadbandMaxPwm = deadbandMax;
+  port->deadbandMinPwm = deadbandMin;
+  port->centerPwm = center;
+  port->minPwm = min;
+  port->configSet = true;
 }
 
 void HAL_GetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle,
                                   int32_t* maxPwm, int32_t* deadbandMaxPwm,
                                   int32_t* centerPwm, int32_t* deadbandMinPwm,
                                   int32_t* minPwm, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  *maxPwm = port->maxPwm;
+  *deadbandMaxPwm = port->deadbandMaxPwm;
+  *deadbandMinPwm = port->deadbandMinPwm;
+  *centerPwm = port->centerPwm;
+  *minPwm = port->minPwm;
 }
 
 void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
                                  HAL_Bool eliminateDeadband, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  port->eliminateDeadband = eliminateDeadband;
 }
 
 HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
                                      int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return false;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+  return port->eliminateDeadband;
 }
 
 void HAL_SetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle,
                                      int32_t microsecondPulseTime,
                                      int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (microsecondPulseTime < 0 ||
+      (microsecondPulseTime != 0xFFFF && microsecondPulseTime >= 4096)) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Pulse time {} out of range. Expect [0-4096) or 0xFFFF",
+                    microsecondPulseTime));
+    return;
+  }
+
+  *status = port->SetPwmMicroseconds(microsecondPulseTime);
 }
 
 void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
                      int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  if (!port->configSet) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+
+  SmartIo* dPort = port.get();
+
+  if (std::isfinite(speed)) {
+    speed = std::clamp(speed, -1.0, 1.0);
+  } else {
+    speed = 0.0;
+  }
+
+  // calculate the desired output pwm value by scaling the speed appropriately
+  int32_t rawValue;
+  if (speed == 0.0) {
+    rawValue = GetCenterPwm(dPort);
+  } else if (speed > 0.0) {
+    rawValue =
+        std::lround(speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
+                    static_cast<double>(GetMinPositivePwm(dPort)));
+  } else {
+    rawValue =
+        std::lround(speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
+                    static_cast<double>(GetMaxNegativePwm(dPort)));
+  }
+
+  if (!((rawValue >= GetMinNegativePwm(dPort)) &&
+        (rawValue <= GetMaxPositivePwm(dPort))) ||
+      rawValue == kPwmDisabled) {
+    *status = HAL_PWM_SCALE_ERROR;
+    return;
+  }
+
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
 }
 
 void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
                         int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  if (!port->configSet) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+  SmartIo* dPort = port.get();
+
+  if (pos < 0.0) {
+    pos = 0.0;
+  } else if (pos > 1.0) {
+    pos = 1.0;
+  }
+
+  // note, need to perform the multiplication below as floating point before
+  // converting to int
+  int32_t rawValue = static_cast<int32_t>(
+      (pos * static_cast<double>(GetFullRangeScaleFactor(dPort))) +
+      GetMinNegativePwm(dPort));
+
+  if (rawValue == kPwmDisabled) {
+    *status = HAL_PWM_SCALE_ERROR;
+    return;
+  }
+
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
 }
 
 void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmDisabled, status);
 }
 
 int32_t HAL_GetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle,
                                         int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  uint16_t microseconds = 0;
+  *status = port->GetPwmMicroseconds(&microseconds);
+  return microseconds;
 }
 
 double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  if (!port->configSet) {
+    *status = INCOMPATIBLE_STATE;
+    return 0;
+  }
+
+  int32_t value = HAL_GetPWMPulseTimeMicroseconds(pwmPortHandle, status);
+  if (*status != 0) {
+    return 0;
+  }
+  SmartIo* dPort = port.get();
+  if (value == kPwmDisabled) {
+    return 0.0;
+  } else if (value > GetMaxPositivePwm(dPort)) {
+    return 1.0;
+  } else if (value < GetMinNegativePwm(dPort)) {
+    return -1.0;
+  } else if (value > GetMinPositivePwm(dPort)) {
+    return static_cast<double>(value - GetMinPositivePwm(dPort)) /
+           static_cast<double>(GetPositiveScaleFactor(dPort));
+  } else if (value < GetMaxNegativePwm(dPort)) {
+    return static_cast<double>(value - GetMaxNegativePwm(dPort)) /
+           static_cast<double>(GetNegativeScaleFactor(dPort));
+  } else {
+    return 0.0;
+  }
 }
 
 double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  if (!port->configSet) {
+    *status = INCOMPATIBLE_STATE;
+    return 0;
+  }
+
+  int32_t value = HAL_GetPWMPulseTimeMicroseconds(pwmPortHandle, status);
+  if (*status != 0) {
+    return 0;
+  }
+  SmartIo* dPort = port.get();
+
+  if (value < GetMinNegativePwm(dPort)) {
+    return 0.0;
+  } else if (value > GetMaxPositivePwm(dPort)) {
+    return 1.0;
+  } else {
+    return static_cast<double>(value - GetMinNegativePwm(dPort)) /
+           static_cast<double>(GetFullRangeScaleFactor(dPort));
+  }
 }
 
 void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
+  // TODO(thad) figure out what this actually means
   return;
 }
 
 void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
                            int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
+  // TODO(thad) not currently supported
   return;
 }
 
 void HAL_SetPWMAlwaysHighMode(HAL_DigitalHandle pwmPortHandle,
                               int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmAlwaysHigh, status);
 }
 
 int32_t HAL_GetPWMLoopTiming(int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
+  // TODO(thad) not currently supported
   return 0;
 }
 
 uint64_t HAL_GetPWMCycleStartTime(int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
+  // TODO(thad) not currently supported
   return 0;
 }
 

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

@@ -8,6 +8,7 @@
 
 namespace hal {
 
+constexpr int32_t kNumSmartIo = 4;
 constexpr int32_t kNumAccumulators = 0;
 constexpr int32_t kNumAnalogTriggers = 0;
 constexpr int32_t kNumAnalogInputs = 8;

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

@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "SmartIo.h"
+
+#include <atomic>
+
+#include "HALInitializer.h"
+#include "SystemServer.h"
+
+namespace hal {
+
+wpi::mutex smartIoMutex;
+DigitalHandleResource<HAL_DigitalHandle, SmartIo, kNumSmartIo>* smartIoHandles;
+
+namespace init {
+void InitializeSmartIo() {
+  static DigitalHandleResource<HAL_DigitalHandle, SmartIo, kNumSmartIo> dcH;
+  smartIoHandles = &dcH;
+}
+}  // namespace init
+
+int32_t SmartIo::InitializeMode(SmartIoMode mode) {
+  auto inst = hal::GetSystemServer();
+
+  nt::PubSubOptions options;
+  options.sendAll = true;
+  options.keepDuplicates = true;
+  options.periodic = 0.005;
+
+  auto channelString = std::to_string(channel);
+
+  modePublisher = inst.GetDoubleTopic("/io/type" + channelString).Publish();
+  getSubscriber = inst.GetDoubleTopic("/io/valread" + channelString)
+                      .Subscribe(0.0, options);
+
+  currentMode = mode;
+  switch (mode) {
+    case SmartIoMode::PWMOutput:
+      modePublisher.Set(3);
+      setPublisher =
+          inst.GetDoubleTopic("/io/valset" + channelString).Publish(options);
+      setPublisher.Set(0);
+      pwmMinPublisher =
+          inst.GetDoubleTopic("/io/pwmmim" + channelString).Publish();
+      pwmMinPublisher.Set(0);
+      pwmMaxPublisher =
+          inst.GetDoubleTopic("/io/pwmmax" + channelString).Publish();
+      pwmMaxPublisher.Set(4096);
+      return 0;
+
+    default:
+
+      return INCOMPATIBLE_STATE;
+  }
+}
+
+int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) {
+  if (currentMode != SmartIoMode::PWMOutput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  // TODO(thad) add support for always on signal
+
+  if (microseconds > 4096) {
+    microseconds = 4096;
+  }
+
+  // Scale from 0-4096 to 0.0-2.0, then to -1.0-1.0
+  setPublisher.Set((microseconds / 2048.0) - 1);
+
+  return 0;
+}
+
+int32_t SmartIo::GetPwmMicroseconds(uint16_t* microseconds) {
+  if (currentMode != SmartIoMode::PWMOutput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  double val = getSubscriber.Get();
+
+  // Get to 0-2, then scale to 0-4096;
+  *microseconds = (val + 1) * 2048;
+
+  return 0;
+}
+
+}  // namespace hal

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

@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include "PortsInternal.h"
+#include "hal/handles/DigitalHandleResource.h"
+#include "hal/handles/HandlesInternal.h"
+#include "networktables/DoubleTopic.h"
+
+namespace hal {
+
+constexpr int32_t kPwmDisabled = 0;
+constexpr int32_t kPwmAlwaysHigh = 0xFFFF;
+
+enum class SmartIoMode {
+  DigitalInput,
+  PWMOutput,
+};
+
+struct SmartIo {
+  uint8_t channel;
+  bool configSet = false;
+  bool eliminateDeadband = false;
+  int32_t maxPwm = 0;
+  int32_t deadbandMaxPwm = 0;
+  int32_t centerPwm = 0;
+  int32_t deadbandMinPwm = 0;
+  int32_t minPwm = 0;
+  std::string previousAllocation;
+  SmartIoMode currentMode{SmartIoMode::DigitalInput};
+  nt::DoublePublisher modePublisher;
+
+  nt::DoublePublisher setPublisher;
+  nt::DoubleSubscriber getSubscriber;
+
+  nt::DoublePublisher pwmMinPublisher;
+  nt::DoublePublisher pwmMaxPublisher;
+
+  int32_t InitializeMode(SmartIoMode mode);
+  int32_t SetPwmMicroseconds(uint16_t microseconds);
+  int32_t GetPwmMicroseconds(uint16_t* microseconds);
+};
+
+extern DigitalHandleResource<HAL_DigitalHandle, SmartIo, kNumSmartIo>*
+    smartIoHandles;
+
+extern wpi::mutex smartIoMutex;
+
+}  // namespace hal