Netzer Encoder

CMakeLists.txt

@@ -172,6 +172,7 @@ add_subdirectory(apps/test-can)
 add_subdirectory(apps/test-mae3)
 add_subdirectory(apps/test-logger)
 add_subdirectory(apps/test-lookup-table)
+add_subdirectory(apps/test-netzer-encoder)
 add_subdirectory(apps/test-adafruitSTEMMA)
 add_subdirectory(apps/test-pid)
 add_subdirectory(apps/test-pwm)

apps/test-netzer-encoder/CMakeLists.txt

@@ -0,0 +1,4 @@
+add_executable(test-netzer-encoder.${TARGET}-board.elf)
+target_sources(test-netzer-encoder.${TARGET}-board.elf PRIVATE src/main.cpp)
+target_link_libraries(test-netzer-encoder.${TARGET}-board.elf PRIVATE EncoderNetzer Logger)
+target_set_firmware_properties(test-netzer-encoder.${TARGET}-board.elf)

apps/test-netzer-encoder/src/main.cpp

@@ -0,0 +1,5 @@
+#include "Netzer.h"
+
+int main() {
+  return 1;
+}

lib/encoder/CMakeLists.txt

@@ -19,3 +19,10 @@ target_sources(EncoderAEAT6012 PRIVATE src/AEAT6012.cpp)
 target_include_directories(EncoderAEAT6012 PUBLIC include)
 target_link_libraries(EncoderAEAT6012 PRIVATE Encoder)
 target_set_mbed_dependency(EncoderAEAT6012)
+
+add_library(EncoderNetzer STATIC)
+target_sources(EncoderNetzer PRIVATE src/Netzer.cpp)
+target_include_directories(EncoderNetzer PUBLIC include)
+target_link_libraries(EncoderNetzer PRIVATE Encoder)
+target_set_mbed_dependency(EncoderNetzer)
+

lib/encoder/include/Netzer.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <mutex>
+
+#include "Encoder.h"
+
+/*
+ *  Netzer DS-25 Datasheet:
+ * 	https://netzerprecision.com/wp-content/uploads/2020/12/DS-25-V02-1.pdf
+ */
+
+namespace Encoder {
+
+class Netzer : public Encoder {
+ public:
+  typedef void (*callback_ptr)(void);
+  Netzer(PinName mosi, PinName miso, PinName sclk, callback_ptr callback, float offset_deg);
+
+  // Return member variables
+  float getAngleDeg() override;
+  float getAngularVelocityDegPerSec() override;
+  uint32_t getRawData();
+
+  bool readAsync(callback_ptr callback);
+
+  // Estimator function
+  void angularVelocityEstimation(void);
+
+  // Async update
+  bool update(callback_ptr callback);
+  // Blocking update
+  [[nodiscard]] bool update() override;
+
+  // Reset encoder values and offset
+  [[nodiscard]] bool reset(void) override;
+
+ private:
+  static constexpr uint32_t DEFAULT_FREQUENCY_HZ      = 20000;  // frequency given by datasheet
+  static constexpr float FLOAT_COMPARE_TOLERANCE      = 1e-6;
+  static constexpr float MOVING_AVERAGE_FILTER_WEIGHT = 0.8;
+
+  static const int WORDS      = 4;    // 4 * 8 bit words = 32 bits
+  const char tx_buffer[WORDS] = {0};  // Initialize dummy tx
+  char rx_buffer[WORDS]       = {0};  // Initialize rx buffer to receive data (16 bits in total)
+  uint32_t m_raw_data;                // 32 bit raw data
+
+  SPI m_spi;                // most, miso, sclk
+  callback_ptr m_callback;  // User callback function to be invoked when an encoder read is complete
+
+  float m_offset_deg;
+  float m_position_deg;
+  float m_angular_velocity_deg_per_sec;
+  uint16_t m_position_raw;
+
+  void priv_spi_callback(int events);
+
+  // Trigger a blocking encoder read
+  bool read(void);
+
+  float rawAbsToDegrees(uint32_t raw);
+
+  Timer m_timer;
+  Mutex m_mutex;
+};
+
+}  // namespace Encoder

lib/encoder/src/Netzer.cpp

@@ -0,0 +1,132 @@
+#include "Netzer.h"
+
+using namespace Encoder;
+
+Netzer::Netzer(PinName mosi, PinName miso, PinName sclk, callback_ptr callback, float offset_deg)
+    : m_spi(mosi, miso, sclk),
+      m_callback(callback),
+      m_offset_deg(offset_deg),
+      m_position_deg(0),
+      m_angular_velocity_deg_per_sec(0),
+      m_position_raw(0) {
+  m_spi.format(16, 3);
+  m_spi.frequency(DEFAULT_FREQUENCY_HZ);
+  m_spi.set_dma_usage(DMA_USAGE_ALWAYS);
+}
+
+bool Netzer::read(void) {
+  int num_bytes = m_spi.write(tx_buffer, WORDS, rx_buffer, WORDS);
+  // TODO: Add check for error bit?
+  if ((num_bytes == WORDS)) {
+    angularVelocityEstimation();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool Netzer::update(callback_ptr callback) {
+  std::scoped_lock<Mutex> lock(m_mutex);
+  m_callback = callback;
+  // return status
+  int status = m_spi.transfer(tx_buffer, WORDS, rx_buffer, WORDS, event_callback_t(this, &Netzer::priv_spi_callback));
+  // status = 0 => SPI transfer started
+  // status = -1 => SPI peripheral is busy
+  return (status == 0);
+}
+
+void Netzer::priv_spi_callback(int events) {
+  angularVelocityEstimation();
+  m_callback();
+}
+
+bool Netzer::reset(void) {
+  std::scoped_lock<Mutex> lock(m_mutex);
+
+  bool success = read();
+  m_offset_deg = rawAbsToDegrees(m_position_raw);
+
+  m_position_deg                 = 0;
+  m_angular_velocity_deg_per_sec = 0;
+
+  return success;
+}
+
+bool Netzer::update() {
+  std::scoped_lock<Mutex> lock(m_mutex);
+  return read();
+}
+
+float Netzer::getAngleDeg(void) {
+  return m_position_deg;
+}
+
+float Netzer::getAngularVelocityDegPerSec(void) {
+  return m_angular_velocity_deg_per_sec;
+}
+
+float Netzer::rawAbsToDegrees(uint32_t raw) {
+  return raw / 131072.0 * 360.0;  // 17 bit resolution, 2^17 = 131072
+}
+
+uint32_t Netzer::getRawData() {
+  return m_raw_data;
+}
+
+// Wrapper function  (taken from AEAT6012 estimator, maybe we can
+// refactor both to use a general estimator function like this?)
+void Netzer::angularVelocityEstimation(void) {
+  // Wrap-around catch for angular velocity estimation:
+  //
+  // If prev_pos = 359 deg and cur_pos = 1 deg, then the change in position would be 1 - 359 = -358
+  //  => This is wrong, since the true change in position is 2
+  //
+  // To prevent this issue, we assume that the sampling rate is high enough and take the smaller of the following
+  // two expressions to be the absolute change in position
+  //   1. abs(cur_pos - prev_pos)
+  //   2. 360 - abs(cur_pos - prev_pos)
+  //
+  // The direction depends on whether we are considering case 1 or case 2:
+  //   1. Direction is positive if cur_pos > prev_pos, negative if cur_pos < prev_pos
+  //   2. Direction is positive if cur_pos < prev_pos, negative if cur_pos > prev_pos
+
+  m_raw_data            = ((((rx_buffer[0] << 16) | rx_buffer[1]) >> 2) & 0x0FFFFFFF);
+  uint32_t raw_position = (m_raw_data >> 8) & 0x1FFFF;
+
+  float curr_pos = rawAbsToDegrees(raw_position) - m_offset_deg;
+  if (curr_pos < 0) {
+    curr_pos += 360;
+  }
+
+  // Get time delta
+  m_timer.stop();
+  float dt = std::chrono::duration_cast<std::chrono::nanoseconds>(m_timer.elapsed_time()).count();
+  m_timer.reset();
+  m_timer.start();
+
+  // Avoid divide by zero
+  if (dt > FLOAT_COMPARE_TOLERANCE) {
+    // Determine change in position
+    float delta_pos_abs = fabs(curr_pos - m_position_deg);
+    int dir             = 1;
+
+    float delta_pos_abs_wrapped = 360 - delta_pos_abs;
+
+    // Choose the wrap-around change in position if it is the smaller one
+    if (delta_pos_abs_wrapped < delta_pos_abs) {
+      dir           = -1;
+      delta_pos_abs = delta_pos_abs_wrapped;
+    }
+
+    dir = dir * (curr_pos > m_position_deg ? 1 : -1);
+
+    // Update angular velocity (apply exponential moving average filter)
+    float new_ang_vel              = dir * delta_pos_abs / dt * 1000000000;
+    m_angular_velocity_deg_per_sec = MOVING_AVERAGE_FILTER_WEIGHT * new_ang_vel +
+                                     (1 - MOVING_AVERAGE_FILTER_WEIGHT) * m_angular_velocity_deg_per_sec;
+  }
+
+  // Update position data
+  m_position_deg = curr_pos;
+  m_position_raw = raw_position;
+}

supported_build_configurations.yaml

@@ -64,6 +64,9 @@ test-adafruitSTEMMA:
 #  - nucleo
   - science
 
+test-netzer-encoder:
+  - nucleo
+
 test-units:
   - nucleo