Add support for automatic SPI transfer engine. (#836)

The SPI Accumulator functions have been moved from HAL to wpilib and rewritten to use the automatic transfer engine.
2026-06-19 00:41:43 +00:00 · 2017-12-13 23:41:37 -08:00
parent d3dd586362
commit 7f074563d0
10 changed files with 1003 additions and 535 deletions
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -12,11 +12,120 @@
 #include <HAL/HAL.h>
 #include <HAL/SPI.h>
 #include <llvm/SmallVector.h>
+#include <support/mutex.h>

+#include "DigitalSource.h"
+#include "Notifier.h"
 #include "WPIErrors.h"

 using namespace frc;

+static constexpr int kAccumulateDepth = 2048;
+
+class SPI::Accumulator {
+ public:
+  Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue,
+              int dataShift, int dataSize, bool isSigned, bool bigEndian)
+      : m_notifier([=]() {
+          std::lock_guard<wpi::mutex> lock(m_mutex);
+          Update();
+        }),
+        m_buf(new uint8_t[xferSize * kAccumulateDepth]),
+        m_validMask(validMask),
+        m_validValue(validValue),
+        m_dataMax(1 << dataSize),
+        m_dataMsbMask(1 << (dataSize - 1)),
+        m_dataShift(dataShift),
+        m_xferSize(xferSize),
+        m_isSigned(isSigned),
+        m_bigEndian(bigEndian),
+        m_port(port) {}
+  ~Accumulator() { delete[] m_buf; }
+
+  void Update();
+
+  Notifier m_notifier;
+  uint8_t* m_buf;
+  wpi::mutex m_mutex;
+
+  int64_t m_value = 0;
+  uint32_t m_count = 0;
+  int32_t m_lastValue = 0;
+
+  int32_t m_center = 0;
+  int32_t m_deadband = 0;
+
+  int32_t m_validMask;
+  int32_t m_validValue;
+  int32_t m_dataMax;      // one more than max data value
+  int32_t m_dataMsbMask;  // data field MSB mask (for signed)
+  uint8_t m_dataShift;    // data field shift right amount, in bits
+  int32_t m_xferSize;     // SPI transfer size, in bytes
+  bool m_isSigned;        // is data field signed?
+  bool m_bigEndian;       // is response big endian?
+  HAL_SPIPort m_port;
+};
+
+void SPI::Accumulator::Update() {
+  bool done;
+  do {
+    done = true;
+    int32_t status = 0;
+
+    // get amount of data available
+    int32_t numToRead =
+        HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status);
+    if (status != 0) return;  // error reading
+
+    // only get whole responses
+    numToRead -= numToRead % m_xferSize;
+    if (numToRead > m_xferSize * kAccumulateDepth) {
+      numToRead = m_xferSize * kAccumulateDepth;
+      done = false;
+    }
+    if (numToRead == 0) return;  // no samples
+
+    // read buffered data
+    HAL_ReadSPIAutoReceivedData(m_port, m_buf, numToRead, 0, &status);
+    if (status != 0) return;  // error reading
+
+    // loop over all responses
+    for (int32_t off = 0; off < numToRead; off += m_xferSize) {
+      // convert from bytes
+      uint32_t resp = 0;
+      if (m_bigEndian) {
+        for (int32_t i = 0; i < m_xferSize; ++i) {
+          resp <<= 8;
+          resp |= m_buf[off + i] & 0xff;
+        }
+      } else {
+        for (int32_t i = m_xferSize - 1; i >= 0; --i) {
+          resp <<= 8;
+          resp |= m_buf[off + i] & 0xff;
+        }
+      }
+
+      // process response
+      if ((resp & m_validMask) == static_cast<uint32_t>(m_validValue)) {
+        // valid sensor data; extract data field
+        int32_t data = static_cast<int32_t>(resp >> m_dataShift);
+        data &= m_dataMax - 1;
+        // 2s complement conversion if signed MSB is set
+        if (m_isSigned && (data & m_dataMsbMask) != 0) data -= m_dataMax;
+        // center offset
+        data -= m_center;
+        // only accumulate if outside deadband
+        if (data < -m_deadband || data > m_deadband) m_value += data;
+        ++m_count;
+        m_lastValue = data;
+      } else {
+        // no data from the sensor; just clear the last value
+        m_lastValue = 0;
+      }
+    }
+  } while (!done);
+}
+
 /**
 * Constructor
 *
@@ -170,6 +279,132 @@ int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
  return retVal;
 }

+/**
+ * Initialize automatic SPI transfer engine.
+ *
+ * Only a single engine is available, and use of it blocks use of all other
+ * chip select usage on the same physical SPI port while it is running.
+ *
+ * @param bufferSize buffer size in bytes
+ */
+void SPI::InitAuto(int bufferSize) {
+  int32_t status = 0;
+  HAL_InitSPIAuto(m_port, bufferSize, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Frees the automatic SPI transfer engine.
+ */
+void SPI::FreeAuto() {
+  int32_t status = 0;
+  HAL_FreeSPIAuto(m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Set the data to be transmitted by the engine.
+ *
+ * Up to 16 bytes are configurable, and may be followed by up to 127 zero
+ * bytes.
+ *
+ * @param dataToSend data to send (maximum 16 bytes)
+ * @param zeroSize number of zeros to send after the data
+ */
+void SPI::SetAutoTransmitData(llvm::ArrayRef<uint8_t> dataToSend,
+                              int zeroSize) {
+  int32_t status = 0;
+  HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
+                             zeroSize, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Start running the automatic SPI transfer engine at a periodic rate.
+ *
+ * InitAuto() and SetAutoTransmitData() must be called before calling this
+ * function.
+ *
+ * @param period period between transfers, in seconds (us resolution)
+ */
+void SPI::StartAutoRate(double period) {
+  int32_t status = 0;
+  HAL_StartSPIAutoRate(m_port, period, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Start running the automatic SPI transfer engine when a trigger occurs.
+ *
+ * InitAuto() and SetAutoTransmitData() must be called before calling this
+ * function.
+ *
+ * @param source digital source for the trigger (may be an analog trigger)
+ * @param rising trigger on the rising edge
+ * @param falling trigger on the falling edge
+ */
+void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
+  int32_t status = 0;
+  HAL_StartSPIAutoTrigger(
+      m_port, source.GetPortHandleForRouting(),
+      (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
+      falling, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Stop running the automatic SPI transfer engine.
+ */
+void SPI::StopAuto() {
+  int32_t status = 0;
+  HAL_StopSPIAuto(m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Force the engine to make a single transfer.
+ */
+void SPI::ForceAutoRead() {
+  int32_t status = 0;
+  HAL_ForceSPIAutoRead(m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+/**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * Blocks until numToRead bytes have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many bytes are available.
+ *
+ * @param buffer buffer where read bytes are stored
+ * @param numToRead number of bytes to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of bytes remaining to be read
+ */
+int SPI::ReadAutoReceivedData(uint8_t* buffer, int numToRead, double timeout) {
+  int32_t status = 0;
+  int32_t val =
+      HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  return val;
+}
+
+/**
+ * Get the number of bytes dropped by the automatic SPI transfer engine due
+ * to the receive buffer being full.
+ *
+ * @return Number of bytes dropped
+ */
+int SPI::GetAutoDroppedCount() {
+  int32_t status = 0;
+  int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  return val;
+}
+
 /**
 * Initialize the accumulator.
 *
@@ -188,29 +423,47 @@ int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
 void SPI::InitAccumulator(double period, int cmd, int xferSize, int validMask,
                          int validValue, int dataShift, int dataSize,
                          bool isSigned, bool bigEndian) {
-  int32_t status = 0;
-  HAL_InitSPIAccumulator(m_port, static_cast<int32_t>(period * 1e6), cmd,
-                         xferSize, validMask, validValue, dataShift, dataSize,
-                         isSigned, bigEndian, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  InitAuto(xferSize * kAccumulateDepth);
+  uint8_t cmdBytes[4] = {0, 0, 0, 0};
+  if (bigEndian) {
+    for (int32_t i = xferSize - 1; i >= 0; --i) {
+      cmdBytes[i] = cmd & 0xff;
+      cmd >>= 8;
+    }
+  } else {
+    cmdBytes[0] = cmd & 0xff;
+    cmd >>= 8;
+    cmdBytes[1] = cmd & 0xff;
+    cmd >>= 8;
+    cmdBytes[2] = cmd & 0xff;
+    cmd >>= 8;
+    cmdBytes[3] = cmd & 0xff;
+  }
+  SetAutoTransmitData(cmdBytes, xferSize - 4);
+  StartAutoRate(period);
+
+  m_accum.reset(new Accumulator(m_port, xferSize, validMask, validValue,
+                                dataShift, dataSize, isSigned, bigEndian));
+  m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2);
 }

 /**
 * Frees the accumulator.
 */
 void SPI::FreeAccumulator() {
-  int32_t status = 0;
-  HAL_FreeSPIAccumulator(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  m_accum.reset(nullptr);
+  FreeAuto();
 }

 /**
 * Resets the accumulator to zero.
 */
 void SPI::ResetAccumulator() {
-  int32_t status = 0;
-  HAL_ResetSPIAccumulator(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  if (!m_accum) return;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->m_value = 0;
+  m_accum->m_count = 0;
+  m_accum->m_lastValue = 0;
 }

 /**
@@ -222,28 +475,28 @@ void SPI::ResetAccumulator() {
 * account when integrating.
 */
 void SPI::SetAccumulatorCenter(int center) {
-  int32_t status = 0;
-  HAL_SetSPIAccumulatorCenter(m_port, center, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  if (!m_accum) return;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->m_center = center;
 }

 /**
 * Set the accumulator's deadband.
 */
 void SPI::SetAccumulatorDeadband(int deadband) {
-  int32_t status = 0;
-  HAL_SetSPIAccumulatorDeadband(m_port, deadband, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  if (!m_accum) return;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->m_deadband = deadband;
 }

 /**
 * Read the last value read by the accumulator engine.
 */
 int SPI::GetAccumulatorLastValue() const {
-  int32_t status = 0;
-  int retVal = HAL_GetSPIAccumulatorLastValue(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-  return retVal;
+  if (!m_accum) return 0;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->Update();
+  return m_accum->m_lastValue;
 }

 /**
@@ -252,10 +505,10 @@ int SPI::GetAccumulatorLastValue() const {
 * @return The 64-bit value accumulated since the last Reset().
 */
 int64_t SPI::GetAccumulatorValue() const {
-  int32_t status = 0;
-  int64_t retVal = HAL_GetSPIAccumulatorValue(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-  return retVal;
+  if (!m_accum) return 0;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->Update();
+  return m_accum->m_value;
 }

 /**
@@ -267,10 +520,10 @@ int64_t SPI::GetAccumulatorValue() const {
 * @return The number of times samples from the channel were accumulated.
 */
 int64_t SPI::GetAccumulatorCount() const {
-  int32_t status = 0;
-  int64_t retVal = HAL_GetSPIAccumulatorCount(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-  return retVal;
+  if (!m_accum) return 0;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->Update();
+  return m_accum->m_count;
 }

 /**
@@ -279,10 +532,11 @@ int64_t SPI::GetAccumulatorCount() const {
 * @return The accumulated average value (value / count).
 */
 double SPI::GetAccumulatorAverage() const {
-  int32_t status = 0;
-  double retVal = HAL_GetSPIAccumulatorAverage(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-  return retVal;
+  if (!m_accum) return 0;
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->Update();
+  if (m_accum->m_count == 0) return 0.0;
+  return static_cast<double>(m_accum->m_value) / m_accum->m_count;
 }

 /**
@@ -295,7 +549,13 @@ double SPI::GetAccumulatorAverage() const {
 * @param count Pointer to the number of accumulation cycles.
 */
 void SPI::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
-  int32_t status = 0;
-  HAL_GetSPIAccumulatorOutput(m_port, &value, &count, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  if (!m_accum) {
+    value = 0;
+    count = 0;
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+  m_accum->Update();
+  value = m_accum->m_value;
+  count = m_accum->m_count;
 }