1
\$\begingroup\$

I have been developing a C++ driver for the incremental rotary encoder (irc) based speed sensor. It is a part of my embedded software project. The driver is basically decomposed into two layers:

  • The first layer consists of the driver for the fpga peripheral for processing the signals generated by the irc sensor. This layer contains the Irc.h, Irc.cpp and IrcConfig.h modules.
  • The second layer consists of the speed sensor model. This layer contains the SpeedSensor.h, SpeedSensor.cpp and SensorConfig.h modules.

As far as the usage of the driver I suppose following:

  1. in the main module the configurations are created at first
  2. the instances of the Irc and SpeedSensor class are created
  3. the driver for the irc peripheral is initialized
  4. the driver for the irc peripheral and the speed sensor model are updated from within the RTOS task in periodic manner

i.e. written in code

FpgaConfig fpga_config = {{IrcConfig::Resolution::kHigh}};
SensorConfig sensor_config = {150u, false};

Irc irc(0, fpga_config.irc);
SpeedSensor speed_sensor(fpga_config, sensor_config, irc,          Irc::Sensor::kSensor_00, 10000);

irc.initialize();

irc.update();
speed_sensor.update();

As far as the implementation

SpeedSensor.h

#ifndef SPEEDSENSOR_H
#define SPEEDSENSOR_H

#include "SensorConfig.h"
#include "FpgaConfig.h"
#include "Irc.h"
#include "Math.h"

/**
 * @brief Model of the speed sensor.
 */
class SpeedSensor
{

public:
  SpeedSensor(
      FpgaConfig &_fpga_configuration,
      SensorConfig &_sensor_configuration,
      Irc &_irc, Irc::Sensor _speed_sensor_id,
      float _execution_period_us);

  /**
   * @brief Code to be refreshed from within the RTOS task.
   */
  void update();

  /**
   * @brief Method returns movement direction.
   * @return movement direction
   */
  Irc::Direction getDirection();

  /**
   * @brief Method returns rotational speed.
   * @return rotational speed in radians per second
   */
  float getRotationalSpeed();

  /**
   * @brief Method returns whether given speed sensor error is
   * active.
   * @param error tested error
   * @return true in case the error is active 
   */
  bool isErrorActive(Irc::Error error);

  /**
   * @brief Method confirms given error.
   * @param error confirmed error
   */
  void confirmError(Irc::Error error);

private:
  enum class State { kWaitForFirstEdge, kCountEdges };
  
  /**< Maximum number of consecutive evaluations with zero edge difference for
   * zero speed determining. */
  static const uint8_t kMaxNoEvaluationsWithZeroEdgeDifference = 40;

  /**< Constant for conversion from radians per second to revolutions per minute. */
  static constexpr float kRadPerSecToRpm = 60.0f / (2 * Math::kPi);

  SensorConfig &sensor_configuration;
  Irc &irc;
  Irc::Sensor speed_sensor_id;

  /**< Number of pulses per one revolution. */
  const uint32_t no_pulses_per_revolution;

  /**< Number of evaluated edges in one period of signal coming from sensor. */
  const uint32_t no_edges_per_period;

  /**< Number of edges in signal coming from the sensor per one revolution. */
  const uint32_t no_edges_per_revolution;

  /**< Used angle resolution. */
  const float angle_resolution_rad;

  State state;
  float execution_period_us;

  uint8_t counter_zero_edge_difference;
  Irc::Direction direction;
  uint8_t no_edges;
  uint8_t no_edges_previous;
  uint8_t edges_difference;
  uint32_t time_stamp;
  uint32_t time_stamp_previous;
  uint32_t time_stamp_difference;
  float time_difference_ns;
  float angle_difference_rad;
  float rotational_speed_rad_per_sec;
  float rotational_speed_rpm;

  /**
   * @brief Method calculates rotational speed based on ratio of the angle 
   * difference and time difference.
   */
  void calculateRotationalSpeed();

  /**
   * @brief Method reads direction of rotation from the driver.
   */
  void readRotationDirection();

  /**
   * @brief Method reads number of signal edges from the driver.
   */
  void readNoEdges();

  /**
   * @brief Method reads time stamp from the driver.
   */
  void readTimeStamp();

  /**
   * @brief Method calculates difference in number of the sensor signal edges.
   */
  void calculateEdgesDifference();

  /**
   * @brief Method calculates derivative of the angle approximated as
   * a ratio of angle difference and time difference.
   */
  void calculateAngleDerivative();

  /**
   * @brief Method transforms difference in edges into the difference in 
   * angle.
   */
  void calculateAngleDifference();

  /**
   * @brief Method calculates difference in time based on difference of the
   * state of the free running counter.
   */
  void calculateTimeDifference();

  /**
   * @brief Method updates speed processing state machine.
   */
  void updateStateMachine();

  /**
   * @brief Method executes the actions related to the WaitForFirstEdge state.
   */
  void doWaitingForFirstEdge();

  /**
   * @brief Method executes the actions related to the CountEdges state.
   */
  void doCountingEdges();

  /**
   * @brief Method converts rotational speed in radians per second to 
   * the revolutions per minute.
   */
  void convertRadiansPerSecondToRevolutionsPerMinute();
};

#endif /* SPEEDSENSOR_H */

SpeedSensor.cpp

#include "SpeedSensor.h"
#include <cstdlib>

SpeedSensor::SpeedSensor(
    FpgaConfig &_fpga_configuration,
    SensorConfig &_sensor_configuration,
    Irc &_irc, Irc::Sensor _speed_sensor_id,
    float _execution_period_us) :
  sensor_configuration(_sensor_configuration),
  irc(_irc),
  speed_sensor_id(_speed_sensor_id),
  no_pulses_per_revolution(
      _sensor_configuration.number_of_pulses_per_revolution),
  no_edges_per_period(
      (_fpga_configuration.irc.resolution ==
       IrcConfig::Resolution::kLow)
          ? IrcConfig::kNoEdgesPerPeriodAtLowResolution
          : IrcConfig::kNoEdgesPerPeriodAtHighResolution),
  no_edges_per_revolution(no_pulses_per_revolution * no_edges_per_period),
  angle_resolution_rad((2 * Math::kPi) / no_edges_per_revolution),
  execution_period_us(_execution_period_us),
  counter_zero_edge_difference(0),
  direction(Irc::Direction::kForward),
  no_edges(0),
  no_edges_previous(0),
  edges_difference(0),
  time_stamp(0),
  time_stamp_previous(0),
  time_stamp_difference(0),
  time_difference_ns(0),
  angle_difference_rad(0),
  rotational_speed_rad_per_sec(0),
  rotational_speed_rpm(0)
{
  state = State::kWaitForFirstEdge;
}

void SpeedSensor::update()
{
  calculateRotationalSpeed();
}

void SpeedSensor::calculateRotationalSpeed()
{
  readRotationDirection();
  readNoEdges();
  readTimeStamp();
  calculateEdgesDifference();
  updateStateMachine();
  convertRadiansPerSecondToRevolutionsPerMinute();
}

Irc::Direction SpeedSensor::getDirection()
{
  return direction;
}

float SpeedSensor::getRotationalSpeed()
{
  return rotational_speed_rad_per_sec;
}

bool SpeedSensor::isErrorActive(Irc::Error error)
{
  return irc.isErrorActive(speed_sensor_id, error);
}

void SpeedSensor::confirmError(Irc::Error error)
{
  irc.confirmError(speed_sensor_id, error);
}

void SpeedSensor::readRotationDirection()
{
  direction = irc.getDirection(speed_sensor_id);
}

void SpeedSensor::readNoEdges()
{
  no_edges = irc.getEdgeCounterState(speed_sensor_id);
}

void SpeedSensor::readTimeStamp()
{
  time_stamp = irc.getTimeStamp(speed_sensor_id);
}

void SpeedSensor::calculateEdgesDifference()
{
  if (direction == Irc::Direction::kForward) {
    edges_difference = no_edges - no_edges_previous;
  } else {
    edges_difference = no_edges_previous - no_edges;
  }
  no_edges_previous = no_edges;
}

void SpeedSensor::calculateAngleDerivative()
{
  calculateAngleDifference();
  calculateTimeDifference();
  // rad/s = rad/ns*10^9
  float angle_derivative =
      (angle_difference_rad / time_difference_ns) * Math::kNsInSec;
  if (sensor_configuration.positive_phase_sequence_produces_positive_speed) {
    rotational_speed_rad_per_sec = angle_derivative;
  } else {
    rotational_speed_rad_per_sec = -angle_derivative;
  }
}

void SpeedSensor::calculateAngleDifference()
{
  if (direction == Irc::Direction::kForward) {
    angle_difference_rad = angle_resolution_rad * edges_difference;
  } else {
    angle_difference_rad = -angle_resolution_rad * edges_difference;
  }
}

void SpeedSensor::calculateTimeDifference()
{
  // time stamp difference modulo 2^24-1
  time_stamp_difference = (time_stamp - time_stamp_previous) & 0x00FFFFFF;
  time_difference_ns = time_stamp_difference *
                        Irc::kTimeStampCounterClockPeriodNs;
  time_stamp_previous = time_stamp;
}

void SpeedSensor::updateStateMachine()
{
  switch (state) {
    case State::kWaitForFirstEdge:
      doWaitingForFirstEdge();
      break;

    case State::kCountEdges:
      doCountingEdges();
      break;
  };
}

void SpeedSensor::doWaitingForFirstEdge()
{
  if (edges_difference > 0) {
    state = State::kCountEdges;
    counter_zero_edge_difference = 0;
    calculateAngleDerivative();
  } else {
    rotational_speed_rad_per_sec = 0;
  }
}

void SpeedSensor::doCountingEdges()
{
  if (edges_difference > 0) {
    counter_zero_edge_difference = 0;
    calculateAngleDerivative();
  } else {
    counter_zero_edge_difference += 1;
    if (counter_zero_edge_difference >=
        kMaxNoEvaluationsWithZeroEdgeDifference) {
      state = State::kWaitForFirstEdge;
      rotational_speed_rad_per_sec = 0;
    } else {
      // minimal rotational speed which can be evaluated by the used algorithm
      // is such a speed which produces exactly one edge between two consecutive
      // calls of the execution loop, the angle per one edge is (2*pi)/(4*Nppr)
      // for high resolution, where Nppr is number of pulses per one revolution,
      // in case this angle is rotated during the execution period time it implies
      // that during one second following angle is rotated (2*pi)/(4*Nppr*execution_period)
      // in case none edge occurred during one execution_period (or even during its integer
      // multiple) it implies that time for rotating the angle corresponding to the angle
      // resolution increases

      // new speed candidate
      float rotational_speed_new_value_candidate =
          (angle_resolution_rad * Math::kUsInSec) /
          (counter_zero_edge_difference * execution_period_us);

      if (rotational_speed_new_value_candidate <
          std::abs(rotational_speed_rad_per_sec)) {
        // only in case new speed candidate is less than current rotational speed
        // it's stated as new rotational speed
        if (rotational_speed_rad_per_sec < 0) {
           rotational_speed_rad_per_sec = - 
           rotational_speed_new_value_candidate;
        } else {
           rotational_speed_rad_per_sec = 
           rotational_speed_new_value_candidate;
        }
        }
      }
    }
  }
}

void SpeedSensor::convertRadiansPerSecondToRevolutionsPerMinute()
{
  rotational_speed_rpm = rotational_speed_rad_per_sec * kRadPerSecToRpm;
}

SensorConfig.h

#ifndef SENSORCONFIG_H
#define SENSORCONFIG_H

#include <cstdint>

/**
 * @brief Container of the speed sensor configuration.
 */
struct SensorConfig
{
  /**< Number of pulses at the speed sensor output per one revolution */
  uint32_t number_of_pulses_per_revolution;
  /**< Stator positive phase sequence u->v->w produces positive speed */
  bool positive_phase_sequence_produces_positive_speed;
};

#endif /* SENSORCONFIG_H */

Irc.h

#ifndef IRC_H
#define IRC_H

#include "IrcConfig.h"
#include <cstddef>
#include <cstdint>

/**
 * @brief Driver for the pair of the incremental rotary encoders
 */
class Irc
{

public:
  /**< Incremental rotary encoder */
  enum class Sensor { kSensor_00, kSensor_01, kNoSensors };

  /**< Evaluated errors */
  enum class Error {
    kDirectionError = 1,
    kTraceAMissingPulseError = 2,
    kTraceBMissingPulseError = 3,
    kTraceAComplementarityError = 4,
    kTraceBComplementarityError = 5
  };

  /**< Drive direction */
  enum class Direction { kBackward, kForward };

  /**< Clock period of the free running time stamp counter. */
  static constexpr float kTimeStampCounterClockPeriodNs = 80.0f;

  Irc(size_t _base_address, const IrcConfig &_config);

  /**
   * @brief Method initializes the peripheral. This method has to be called
   * as the very first method after driver instance creation.
   */
  void initialize();

  /**
   * @brief Method activates reset of the peripheral.
   */
  void activateReset();

  /**
   * @brief Method deactivates reset of the peripheral.
   */
  void deactivateReset();

  /**
   * @brief Method increases the angle resolution based on configuration modification
   * exploiting all four edges per period of the output signals coming from the
   * incremental rotary encoder.
   */ 
  void increaseResolution();

  /**
   * @brief Method reduces the angle resolution based on configuration modification
   * exploiting only one edge per period of the output signals coming from the
   * incremental rotary encoder.
   */ 
  void reduceResolution();

  /**
   * @brief Method returns current resolution configuration.
   * @return current resolution configuration
   */ 
  IrcConfig::Resolution getResolution();

  /**
   * @brief Method is intended to be called from within the RTOS task.
   */
  void update();

  /**
   * @brief Method tests whether given sensor evaluated given error.
   * @param sensor tested sensor
   * @param error tested error
   * @return true in case error has occurred
   */
  bool isErrorActive(Sensor sensor, Error error) const;

  /**
   * @brief Method confirms given error at given sensor.
   * @param sensor sensor at which the error is being confirmed
   * @param error confirmed error
   */
  void confirmError(Sensor sensor, Error error);

  /**
   * @brief Method returns direction of rotation detected by given sensor.
   * @param sensor sensor whose direction of rotation is requested
   * @return direction of rotation detected by given sensor
   */
  Direction getDirection(Sensor sensor) const;

  /**
   * @brief Method returns status of the rising edges counter for given sensor. 
   * The rising edge occurs with each valid change of state at the encoder 
   * traces i.e. there are four state changes per encoder pulse.
   * @param sensor sensor for which the counter status is requested
   * @return status of the rising edges counter for given sensor
   */
  uint8_t getEdgeCounterState(Sensor sensor) const;

  /**
   * @brief Method returns time stamp expressed as the state of the 24 bits free 
   * running counter for given sensor. This time stamp accompanies the status of the 
   * rising edge counter accessible via @see getEdgeCounterState method call.
   * @param sensor sensor for which the counter status is requested
   * @return time stamp expressed as the state of the 24 bits free running counter
   */
  uint32_t getTimeStamp(Sensor sensor) const;

private:
  /**< Control register */
  struct ControlReg
  {
    uint32_t reset_bit : 1;
    uint32_t reduce_resolution_bit : 1;
  };

  /**< Irc register map  */
  struct IrcRegs
  {
    volatile ControlReg control_reg;
    volatile uint32_t status_reg;
    volatile uint32_t sensor_regs[static_cast<uint8_t>(Sensor::kNoSensors)];
  };

  IrcRegs *regs;
  const IrcConfig &config;

  uint32_t status_reg_mirror;
  uint32_t sensor_regs_mirror[static_cast<uint8_t>(Sensor::kNoSensors)];
};

#endif /* IRC_H */

Irc.cpp

#include "irc.h"

#include <new>

Irc::Irc(size_t _base_address, const IrcConfig &_config) : config(_config)
{
}

void Irc::initialize()
{
  
}

void Irc::activateReset()
{
  
}

void Irc::deactivateReset()
{
  
}

void Irc::increaseResolution()
{
  
}

void Irc::reduceResolution()
{
  
}

IrcConfig::Resolution Irc::getResolution()
{
    return IrcConfig::Resolution::kLow;
}

void Irc::update()
{
  
}

bool Irc::isErrorActive(Sensor sensor, Error error) const
{
    return false;
}

void Irc::confirmError(Sensor sensor, Error error)
{
  
}

Irc::Direction Irc::getDirection(Sensor sensor) const
{
    return Irc::Direction::kForward;
}

uint8_t Irc::getEdgeCounterState(Sensor sensor) const
{
    return 0;
}

uint32_t Irc::getTimeStamp(Sensor sensor) const
{
    return 0;
}

IrcConfig.h

#ifndef IRCCONFIG_H
#define IRCCONFIG_H

#include <cstdint>

/**
 * @brief Configuration of the peripheral for processing of the 
 * signals from the incremental rotary encoder.
 */
struct IrcConfig
{
  /**< Number of processed edges of the signal based on selected resolution */
  static const uint32_t kNoEdgesPerPeriodAtLowResolution = 1;
  static const uint32_t kNoEdgesPerPeriodAtHighResolution = 4;

  enum class Resolution {
    kHigh, /**< Peripheral uses four edges per period */
    kLow /**< Peripheral uses only one edge per period */
  };

  /**< Resolution configuration */
  Resolution resolution;
};

#endif /* IRCCONFIG_H */

FpgaConfig.h

#ifndef FPGACONFIG_H
#define FPGACONFIG_H

#include "IrcConfig.h"

/**
 * @brief Container containing fpga configuration.
 */
struct FpgaConfig
{
  IrcConfig irc;
};

#endif /* FPGACONFIG_H */

Math.h

#ifndef MATH_H
#define MATH_H

class Math 
{
  public:
  
  /**< Ludolph number */
  static constexpr float kPi = 3.14f;
  
  /**< Number of microseconds in one second */
  static constexpr float kUsInSec = 1e6f;
  
  /**< Number of nanoseconds in one second */
  static constexpr float kNsInSec = 1e9f;
};

#endif /* MATH_H */
\$\endgroup\$
7
  • \$\begingroup\$ What compiler, what C++ standard? \$\endgroup\$
    – Reinderien
    Oct 27, 2022 at 11:49
  • \$\begingroup\$ The compiler is gcc v4.6.3 and the standard is C++ 11. \$\endgroup\$
    – L3sek
    Oct 27, 2022 at 12:02
  • \$\begingroup\$ Why 11 and not 20? \$\endgroup\$
    – Reinderien
    Oct 27, 2022 at 12:32
  • \$\begingroup\$ Is it a problem for the code review purposes? \$\endgroup\$
    – L3sek
    Oct 27, 2022 at 12:54
  • \$\begingroup\$ It's not a review problem, but it's somewhat of a design problem. \$\endgroup\$
    – Reinderien
    Oct 27, 2022 at 14:35

1 Answer 1

1
\$\begingroup\$

Some questions about the FPGA interface

There are some questions that I have when I look at the interface you are using. You mention an FPGA that presumely is doing the quadrature decoding and maintaining a counter for the current position. I was wondering why it would support both 1X and 4X decoding, when 4X decoding is superior, but then I noticed that getEdgeCounterState() returns an 8-bit number. Does this mean the FPGA only has an 8-bit counter for the position? That seems very small, why not have a 32-bit counter? 8 bits is small enough that at high rotational speeds, the counter might wrap between reads from the microcontroller. So I would either:

  1. Reprogram the FPGA to have a larger counter, or
  2. If you cannot change the FPGA, think carefully about how to handle the counter wrapping.

I'm also surprised by the fact that there is a separate getDirection() function. What if the direction is rapidly changing between reads? Or what if the direction changes between a call to getEdgeCounterState and getDirection()?

There is a free running counter you use for timing purposes, but is this counter read at the same time as the edge counter is read out? If not, the calculated speed might not be correct.

Accuracy

What are you going to use the speed sensor for, and how accurate does it need to be? Assuming the hardware is perfect (for example, a OCXO is used as a clock source), you still have some sources of inaccuracy due to the discrete nature of counting pulses from the encoder, and from the math you do in your code. The more often you call update(), the less accurate the speed will be, because the difference between timestamp values will be smaller, and the rounding error will get larger and larger.

Then you have your conversion to a float value for the speed in rad/s, which uses kPi, which is defined to be 3.14. Really? This is worse than the ancient Egyptian's approximation of 22/7. There is no reason not to use a better value for \$\pi\$ here, like:

  • C++20's std::numbers::pi
  • M_PI from <cmath> (might not work on all platforms)
  • 4 * std::atan(1) (might not work in a constexpr context)
  • Just write more digits: 3.14159265359

Pulses vs edges vs quadrature

/* @brief Method returns status of the rising edges counter for given sensor. 
 * The rising edge occurs with each valid change of state at the encoder 
 * traces i.e. there are four state changes per encoder pulse.

The text here is not accurate, "pulses" and "edges" are used in a sloppy way that will confuse the reader. A single encoder has two output signals, that together form a quadrature output. Each of the two output signals will produce pulses and has edges, but it's not accurate to say that the encoder as a whole produces "X pulses per revolution". It's better to say "(quadrature) state changes" per revolution. You already mention state changes in the comments:

The rising edge occurs with each valid change of state at the encoder traces

Maybe that "rising edge" really exists somewhere inside the FPGA, but there is no such externally visible signal. It would be better to say that a "count" occurs at each valid change of state. After all, there is a counter involved here.

Also note that the comments here make it sound that 4X decoding is always done, but you could configure the FPGA to do 1X decoding, so that would mean one count per 4 valid state changes.

Don't store temporary values in member variables

There are a lot of member variables in SpeedSensor.h that are not really necessary. Assuming you still want to have the public API such that you call update() to calculate the speed, and getRotationalSpeed() to get the current speed value, then the only things you need to store after state are no_edges_previous and time_stamp_previous to be able to calculate the difference in position and time, and direction and rotational_speed_rad_per_sec. You can do this by having more functions return the values they calculate instead of storing them in member variables:

void SpeedSensor::update()
{
    rotational_speed_rad_per_sec = calculateRotationalSpeed();
}

float SpeedSensor::calculateRotationalSpeed()
{
    uint8_t no_edges = readNoEdges();
    uint32_t time_stamp = readTimeStamp();
    uint8_t edges_difference = calculateEdgesDifference(no_edges_previous, no_edges);
    uint32_t time_stamp_difference = calculateTimeStampDifference(time_stamp_previous, time_stamp);
    ...
    no_edges_previous = no_edges;
    time_stamp_previous = time_stamp;
    return calculateRadiansPerSecond(edges_difference, time_stamp_difference);
}
...

Also avoid calculating and storing things you are not using to begin with, like the rotational speed in RPM. Especially on an embedded device, RAM is a limited resource, so don't waste it.

Use volatile struct

I see you have a struct IrcRegs that contains volatile members, but then you have a mirror of the registors which indeed should not be volatile. However, now you had to duplicate some code. Consider not making the members of IrcRegs volatile, but rather make the whole struct volatile where desired. Consider:

struct IrcRegs
{
    ControlReg control;
    uint32_t status;
    uint32_t sensors[static_cast<uint8_t>(Sensor::kNoSensors)];
};

volatile IrcRegs *regs;
IrcRegs regs_mirror;

Consider using std::chrono

If possible, make use of C++'s date and time utilities to store time stamps and durations.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.