[hal] Digital IO SystemCore implementation (#7621)

developerRobot/build.gradle

@@ -71,12 +71,18 @@ deploy {
             directory = '/home/systemcore'
             maxChannels = 4
             locations {
-                ssh(SshDeployLocation) {
+                mdns(SshDeployLocation) {
                     address = "limelight.local"
                     user = 'systemcore'
                     password = 'systemcore'
                     ipv6 = false
                 }
+                usb(SshDeployLocation) {
+                    address = "172.28.0.1"
+                    user = 'systemcore'
+                    password = 'systemcore'
+                    ipv6 = false
+                }
             }
 
             timeout = 7

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

@@ -11,6 +11,7 @@
 #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"
@@ -29,15 +30,69 @@ HAL_DigitalHandle HAL_InitializeDIOPort(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 DIO", 0,
+                                     kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port =
+      smartIoHandles->Allocate(channel, HAL_HandleEnum::DIO, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                       kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(input ? SmartIoMode::DigitalInput
+                                       : SmartIoMode::DigitalOutput);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::DIO);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
 }
 
 HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
-  return channel < kNumDigitalChannels && channel >= 0;
+  return channel < kNumSmartIo && channel >= 0;
 }
 
-void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {}
+void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    return;
+  }
+
+  smartIoHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
+
+  // 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("DIO handle free timeout");
+      std::fflush(stdout);
+      break;
+    }
+    std::this_thread::yield();
+  }
+}
 
 void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
 }
@@ -74,24 +129,54 @@ void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
 
 void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
                 int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SetDigitalOutput(value);
 }
 
 void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
                          int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SwitchDioDirection(input);
 }
 
 HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return false;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  bool ret = false;
+  *status = port->GetDigitalInput(&ret);
+  return ret;
 }
 
 HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return false;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  switch (port->currentMode) {
+    case SmartIoMode::DigitalInput:
+      return true;
+    case SmartIoMode::DigitalOutput:
+      return false;
+    default:
+      *status = INCOMPATIBLE_STATE;
+      return false;
+  }
 }
 
 void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,

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

@@ -99,7 +99,7 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
 
   port->channel = channel;
 
-  *status = port->InitializeMode(SmartIoMode::PWMOutput);
+  *status = port->InitializeMode(SmartIoMode::PwmOutput);
   if (*status != 0) {
     smartIoHandles->Free(handle, HAL_HandleEnum::PWM);
     return HAL_kInvalidHandle;
@@ -372,18 +372,48 @@ double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
 }
 
 void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
-  // TODO(thad) figure out what this actually means
-  return;
+  HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, 0, status);
 }
 
 void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
                            int32_t* status) {
-  // TODO(thad) not currently supported
-  return;
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  switch (squelchMask) {
+    case 0:
+      *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k5ms);
+      break;
+    case 1:
+    case 2:
+      *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k10ms);
+      break;
+    case 3:
+      *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k20ms);
+      break;
+    default:
+      *status = PARAMETER_OUT_OF_RANGE;
+      return;
+  }
 }
 
 void HAL_SetPWMAlwaysHighMode(HAL_DigitalHandle pwmPortHandle,
                               int32_t* status) {
+  // Always high is going to have to use a 2ms period
+  auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k2ms);
+  if (*status != 0) {
+    return;
+  }
+
   HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmAlwaysHigh, status);
 }
 

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

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

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

@@ -34,29 +34,90 @@ int32_t SmartIo::InitializeMode(SmartIoMode mode) {
 
   modePublisher = inst.GetIntegerTopic(subTableString + "type").Publish();
   getSubscriber =
-      inst.GetIntegerTopic(subTableString + "valget").Subscribe(0.0, options);
+      inst.GetIntegerTopic(subTableString + "valget").Subscribe(0, options);
+  frequencySubscriber =
+      inst.GetIntegerTopic(subTableString + "freqget").Subscribe(0, options);
+  setPublisher =
+      inst.GetIntegerTopic(subTableString + "valset").Publish(options);
+  periodPublisher =
+      inst.GetIntegerTopic(subTableString + "periodset").Publish(options);
 
   currentMode = mode;
   switch (mode) {
-    case SmartIoMode::PWMOutput:
-      modePublisher.Set(4);
-      setPublisher =
-          inst.GetIntegerTopic(subTableString + "valset").Publish(options);
+    // These need to set a 0 output
+    case SmartIoMode::DigitalOutput:
+    case SmartIoMode::PwmOutput:
       setPublisher.Set(0);
-      return 0;
+      break;
+
+    // These don't need to set any value
+    case SmartIoMode::DigitalInput:
+    case SmartIoMode::AnalogInput:
+    case SmartIoMode::PwmInput:
+    case SmartIoMode::SingleCounterRising:
+    case SmartIoMode::SingleCounterFalling:
+      break;
 
     default:
 
       return INCOMPATIBLE_STATE;
   }
+
+  modePublisher.Set(static_cast<int>(mode));
+  return 0;
 }
 
-int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) {
-  if (currentMode != SmartIoMode::PWMOutput) {
+int32_t SmartIo::SwitchDioDirection(bool input) {
+  if (currentMode != SmartIoMode::DigitalInput &&
+      currentMode != SmartIoMode::DigitalOutput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  modePublisher.Set(input ? 0 : 1);
+  currentMode = input ? SmartIoMode::DigitalInput : SmartIoMode::DigitalOutput;
+  return 0;
+}
+
+int32_t SmartIo::SetDigitalOutput(bool value) {
+  if (currentMode != SmartIoMode::DigitalInput &&
+      currentMode != SmartIoMode::DigitalOutput) {
+    return INCOMPATIBLE_STATE;
+  }
+  setPublisher.Set(value ? 255.0 : 0.0);
+  return 0;
+}
+
+int32_t SmartIo::GetDigitalInput(bool* value) {
+  if (currentMode != SmartIoMode::DigitalInput &&
+      currentMode != SmartIoMode::DigitalOutput) {
+    return INCOMPATIBLE_STATE;
+  }
+  *value = getSubscriber.Get() != 0;
+  return 0;
+}
+
+int32_t SmartIo::SetPwmOutputPeriod(PwmOutputPeriod period) {
+  if (currentMode != SmartIoMode::PwmOutput) {
     return INCOMPATIBLE_STATE;
   }
 
-  // TODO(thad) add support for always on signal
+  switch (period) {
+    case PwmOutputPeriod::k20ms:
+    case PwmOutputPeriod::k10ms:
+    case PwmOutputPeriod::k5ms:
+    case PwmOutputPeriod::k2ms:
+      periodPublisher.Set(static_cast<int>(period));
+      return 0;
+
+    default:
+      return PARAMETER_OUT_OF_RANGE;
+  }
+}
+
+int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) {
+  if (currentMode != SmartIoMode::PwmOutput) {
+    return INCOMPATIBLE_STATE;
+  }
 
   if (microseconds > 4095) {
     microseconds = 4095;
@@ -68,7 +129,7 @@ int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) {
 }
 
 int32_t SmartIo::GetPwmMicroseconds(uint16_t* microseconds) {
-  if (currentMode != SmartIoMode::PWMOutput) {
+  if (currentMode != SmartIoMode::PwmOutput) {
     return INCOMPATIBLE_STATE;
   }
 

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

@@ -17,8 +17,20 @@ constexpr int32_t kPwmDisabled = 0;
 constexpr int32_t kPwmAlwaysHigh = 0xFFFF;
 
 enum class SmartIoMode {
-  DigitalInput,
-  PWMOutput,
+  DigitalInput = 0,
+  DigitalOutput,
+  AnalogInput,
+  PwmInput,
+  PwmOutput,
+  SingleCounterRising,
+  SingleCounterFalling,
+};
+
+enum class PwmOutputPeriod {
+  k20ms = 0,
+  k10ms,
+  k5ms,
+  k2ms,
 };
 
 struct SmartIo {
@@ -37,7 +49,17 @@ struct SmartIo {
   nt::IntegerPublisher setPublisher;
   nt::IntegerSubscriber getSubscriber;
 
+  nt::IntegerPublisher periodPublisher;
+  nt::IntegerSubscriber frequencySubscriber;
+
   int32_t InitializeMode(SmartIoMode mode);
+  int32_t SwitchDioDirection(bool input);
+
+  int32_t SetDigitalOutput(bool value);
+  int32_t GetDigitalInput(bool* value);
+
+  int32_t SetPwmOutputPeriod(PwmOutputPeriod period);
+
   int32_t SetPwmMicroseconds(uint16_t microseconds);
   int32_t GetPwmMicroseconds(uint16_t* microseconds);
 };