Initial commit of cross connect integration test project (#3434)

Adding as a separate project so current integration tests stay working.

crossConnIntegrationTests/src/main/native/cpp/DIOTests.cpp

@@ -0,0 +1,101 @@
+// 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 <hal/DIO.h>
+#include <wpi/SmallVector.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class DIOTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+TEST_P(DIOTest, TestDIOCross) {
+  auto param = GetParam();
+  int32_t status = 0;
+  DIOHandle first{param.first, false, &status};
+  ASSERT_EQ(0, status);
+  DIOHandle second{param.second, true, &status};
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(first, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_FALSE(HAL_GetDIO(second, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(first, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(second, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIODirection(first, true, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetDIODirection(second, false, &status);
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(second, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(second, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+}
+
+TEST(DIOTest, TestAllocateAll) {
+  wpi::SmallVector<DIOHandle, 32> dioHandles;
+  for (int i = 0; i < HAL_GetNumDigitalChannels(); i++) {
+    int32_t status = 0;
+    dioHandles.emplace_back(i, true, &status);
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(DIOTest, TestMultipleAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(0, true, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  DIOHandle handle2(0, true, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(DIOTest, TestOverAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(HAL_GetNumDigitalChannels(), true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(DIOTest, TestUnderAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(-1, true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(DIOTest, TestCrossAllocationFails) {
+  int32_t status = 0;
+  PWMHandle pwmHandle(10, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+  DIOHandle handle(10, true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+INSTANTIATE_TEST_SUITE_P(DIOCrossConnectsTest, DIOTest,
+                         ::testing::ValuesIn(DIOCrossConnects));

crossConnIntegrationTests/src/main/native/cpp/Main.cpp

@@ -0,0 +1,10 @@
+// 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 "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

crossConnIntegrationTests/src/main/native/cpp/PWMTests.cpp

@@ -0,0 +1,219 @@
+// 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 <atomic>
+#include <thread>
+
+#include <hal/HAL.h>
+#include <wpi/SmallVector.h>
+#include <wpi/condition_variable.h>
+#include <wpi/priority_mutex.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class PWMTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+struct InterruptCheckData {
+  wpi::SmallVector<uint64_t, 8> risingStamps;
+  wpi::SmallVector<uint64_t, 8> fallingStamps;
+  wpi::priority_mutex mutex;
+  wpi::condition_variable cond;
+  HAL_InterruptHandle handle;
+};
+
+// TODO switch this to DMA
+void TestTiming(int squelch, std::pair<int, int> param) {
+  // Initialize interrupt
+  int32_t status = 0;
+  InterruptHandle interruptHandle(&status);
+  // Ensure we have a valid interrupt handle
+  ASSERT_NE(interruptHandle, HAL_kInvalidHandle);
+
+  status = 0;
+  PWMHandle pwmHandle(param.first, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+
+  // Ensure our PWM is disabled, and set up properly
+  HAL_SetPWMRaw(pwmHandle, 0, &status);
+  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMPeriodScale(pwmHandle, squelch, &status);
+
+  unsigned int checkPeriod = 0;
+  switch (squelch) {
+    case (0):
+      checkPeriod = 5050;
+      break;
+    case (1):
+      checkPeriod = 10100;
+      break;
+    case (3):
+      checkPeriod = 20200;
+      break;
+  }
+
+  status = 0;
+  DIOHandle dioHandle(param.second, true, &status);
+  ASSERT_NE(dioHandle, HAL_kInvalidHandle);
+
+  InterruptCheckData interruptData;
+  interruptData.handle = interruptHandle;
+
+  // Can use any type for the interrupt handle
+  HAL_RequestInterrupts(interruptHandle, dioHandle,
+                        HAL_AnalogTriggerType::HAL_Trigger_kInWindow, &status);
+
+  HAL_SetInterruptUpSourceEdge(interruptHandle, true, true, &status);
+
+  // Loop to test 5 speeds
+  for (unsigned int i = 1000; i < 2100; i += 250) {
+    interruptData.risingStamps.clear();
+    interruptData.fallingStamps.clear();
+
+    std::atomic_bool runThread{true};
+
+    status = 0;
+    std::thread interruptThread([&]() {
+      while (runThread) {
+        int32_t threadStatus = 0;
+        auto mask =
+            HAL_WaitForInterrupt(interruptHandle, 5, true, &threadStatus);
+
+        if ((mask & 0x100) == 0x100 && interruptData.risingStamps.size() == 0 &&
+            interruptData.fallingStamps.size() == 0) {
+          // Falling edge at start of tracking. Skip
+          continue;
+        }
+
+        int32_t status = 0;
+        if ((mask & 0x1) == 0x1) {
+          auto ts = HAL_ReadInterruptRisingTimestamp(interruptHandle, &status);
+          // Rising Edge
+          interruptData.risingStamps.push_back(ts);
+        } else if ((mask & 0x100) == 0x100) {
+          auto ts = HAL_ReadInterruptFallingTimestamp(interruptHandle, &status);
+          // Falling Edge
+          interruptData.fallingStamps.push_back(ts);
+        }
+
+        if (interruptData.risingStamps.size() >= 4 &&
+            interruptData.fallingStamps.size() >= 4) {
+          interruptData.cond.notify_all();
+          runThread = false;
+          break;
+        }
+      }
+    });
+
+    // Ensure our interrupt actually got created correctly.
+    ASSERT_EQ(status, 0);
+    HAL_SetPWMSpeed(pwmHandle, (i - 1000) / 1000.0, &status);
+    ASSERT_EQ(status, 0);
+    {
+      std::unique_lock<wpi::priority_mutex> lock(interruptData.mutex);
+      // Wait for lock
+      // TODO: Add Timeout
+      auto timeout = interruptData.cond.wait_for(lock, std::chrono::seconds(2));
+      if (timeout == std::cv_status::timeout) {
+        runThread = false;
+        if (interruptThread.joinable()) {
+          interruptThread.join();
+        }
+        ASSERT_TRUE(false);  // Exit test as failure on timeout
+      }
+    }
+
+    HAL_SetPWMRaw(pwmHandle, 0, &status);
+
+    // Ensure our interrupts have the proper counts
+    ASSERT_EQ(interruptData.risingStamps.size(),
+              interruptData.fallingStamps.size());
+
+    ASSERT_GT(interruptData.risingStamps.size(), 0u);
+
+    ASSERT_EQ(interruptData.risingStamps.size() % 2, 0u);
+    ASSERT_EQ(interruptData.fallingStamps.size() % 2, 0u);
+
+    for (size_t j = 0; j < interruptData.risingStamps.size(); j++) {
+      uint64_t width =
+          interruptData.fallingStamps[j] - interruptData.risingStamps[j];
+      ASSERT_NEAR(width, i, 10);
+    }
+
+    for (unsigned int j = 0; j < interruptData.risingStamps.size() - 1; j++) {
+      uint64_t period =
+          interruptData.risingStamps[j + 1] - interruptData.risingStamps[j];
+      ASSERT_NEAR(period, checkPeriod, 10);
+    }
+    runThread = false;
+    if (interruptThread.joinable()) {
+      interruptThread.join();
+    }
+  }
+}
+
+TEST_P(PWMTest, TestTiming4x) {
+  auto param = GetParam();
+  TestTiming(3, param);
+}
+
+TEST_P(PWMTest, TestTiming2x) {
+  auto param = GetParam();
+  TestTiming(1, param);
+}
+
+TEST_P(PWMTest, TestTiming1x) {
+  auto param = GetParam();
+  TestTiming(0, param);
+}
+
+TEST(PWMTest, TestAllocateAll) {
+  wpi::SmallVector<PWMHandle, 21> pwmHandles;
+  for (int i = 0; i < HAL_GetNumPWMChannels(); i++) {
+    int32_t status = 0;
+    pwmHandles.emplace_back(PWMHandle(i, &status));
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(PWMTest, TestMultipleAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(0, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  PWMHandle handle2(0, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(PWMTest, TestOverAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(HAL_GetNumPWMChannels(), &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(PWMTest, TestUnderAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(-1, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(PWMTest, TestCrossAllocationFails) {
+  int32_t status = 0;
+  DIOHandle dioHandle(10, true, &status);
+  ASSERT_NE(dioHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+  PWMHandle handle(10, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+INSTANTIATE_TEST_SUITE_P(PWMCrossConnectTests, PWMTest,
+                         ::testing::ValuesIn(PWMCrossConnects));

crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp

@@ -0,0 +1,70 @@
+// 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 <cstdlib>
+#include <thread>
+
+#include <fmt/core.h>
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+#include "mockds/MockDS.h"
+
+using namespace std::chrono_literals;
+
+class TestEnvironment : public testing::Environment {
+  bool m_alreadySetUp = false;
+  MockDS m_mockDS;
+
+ public:
+  TestEnvironment() {
+    // Only set up once. This allows gtest_repeat to be used to automatically
+    // repeat tests.
+    if (m_alreadySetUp) {
+      return;
+    }
+    m_alreadySetUp = true;
+
+    if (!HAL_Initialize(500, 0)) {
+      fmt::print(stderr, "FATAL ERROR: HAL could not be initialized\n");
+      std::exit(-1);
+    }
+
+    m_mockDS.Start();
+
+    // This sets up the network communications library to enable the driver
+    // station. After starting network coms, it will loop until the driver
+    // station returns that the robot is enabled, to ensure that tests will be
+    // able to run on the hardware.
+    HAL_ObserveUserProgramStarting();
+
+    fmt::print("Started coms\n");
+
+    int enableCounter = 0;
+
+    auto checkEnabled = []() {
+      HAL_ControlWord controlWord;
+      std::memset(&controlWord, 0, sizeof(controlWord));
+      HAL_GetControlWord(&controlWord);
+      return controlWord.enabled && controlWord.dsAttached;
+    };
+    while (!checkEnabled()) {
+      if (enableCounter > 50) {
+        // Robot did not enable properly after 5 seconds.
+        // Force exit
+        fmt::print(stderr, " Failed to enable. Aborting\n");
+        std::terminate();
+      }
+
+      std::this_thread::sleep_for(100ms);
+
+      fmt::print("Waiting for enable: {}\n", enableCounter++);
+    }
+  }
+
+  ~TestEnvironment() override { m_mockDS.Stop(); }
+};
+
+testing::Environment* const environment =
+    testing::AddGlobalTestEnvironment(new TestEnvironment);

crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp

@@ -0,0 +1,86 @@
+// 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 "MockDS.h"
+
+#include <stdint.h>
+
+#include <string_view>
+
+#include <fmt/core.h>
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/UDPClient.h>
+
+static void LoggerFunc(unsigned int level, const char* file, unsigned int line,
+                       const char* msg) {
+  if (level == 20) {
+    fmt::print(stderr, "DS: {}\n", msg);
+    return;
+  }
+
+  std::string_view levelmsg;
+  if (level >= 50) {
+    levelmsg = "CRITICAL";
+  } else if (level >= 40) {
+    levelmsg = "ERROR";
+  } else if (level >= 30) {
+    levelmsg = "WARNING";
+  } else {
+    return;
+  }
+  fmt::print(stderr, "DS: {}: {} ({}:{})\n", levelmsg, msg, file, line);
+}
+
+static void generateEnabledDsPacket(wpi::SmallVectorImpl<uint8_t>& data,
+                                    uint16_t sendCount) {
+  data.clear();
+  data.push_back(sendCount >> 8);
+  data.push_back(sendCount);
+  data.push_back(0x01);  // general data tag
+  data.push_back(0x04);  // teleop enabled
+  data.push_back(0x10);  // normal data request
+  data.push_back(0x00);  // red 1 station
+}
+
+void MockDS::Start() {
+  if (m_active) {
+    return;
+  }
+  m_active = true;
+  m_thread = std::thread([&]() {
+    wpi::Logger logger(LoggerFunc);
+    wpi::UDPClient client(logger);
+    client.start();
+    auto timeout_time = hal::fpga_clock::now();
+    int initCount = 0;
+    uint16_t sendCount = 0;
+    wpi::SmallVector<uint8_t, 8> data;
+    while (m_active) {
+      // Keep 20ms intervals, and increase time to next interval
+      auto current = hal::fpga_clock::now();
+      while (timeout_time <= current) {
+        timeout_time += std::chrono::milliseconds(20);
+      }
+      std::this_thread::sleep_until(timeout_time);
+      generateEnabledDsPacket(data, sendCount++);
+      // ~10 disabled packets are required to make the robot actually enable
+      // 1 is definitely not enough.
+      if (initCount < 10) {
+        initCount++;
+        data[3] = 0;
+      }
+      client.send(data, "127.0.0.1", 1110);
+    }
+    client.shutdown();
+  });
+}
+
+void MockDS::Stop() {
+  m_active = false;
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
+}

crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.h

@@ -0,0 +1,23 @@
+// 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 <atomic>
+#include <thread>
+
+class MockDS {
+ public:
+  MockDS() = default;
+  ~MockDS() { Stop(); }
+  MockDS(const MockDS& other) = delete;
+  MockDS& operator=(const MockDS& other) = delete;
+
+  void Start();
+  void Stop();
+
+ private:
+  std::thread m_thread;
+  std::atomic_bool m_active{false};
+};