速度传感器驱动程序的实现

Question

我一直在开发一个基于增量旋转编码器的速度传感器的C++驱动程序。这是我的嵌入式软件项目的一部分。驱动程序基本上分解为两层：

• 第一层由fpga外设的驱动程序组成，用于处理irc传感器产生的信号。该层包含Irc.h、Irc.cpp和IrcConfig.h模块。
• 第二层由速度传感器模型组成。该层包含SpeedSensor.h、SpeedSensor.cpp和SensorConfig.h模块。

至于驱动程序的使用，我想如下：

1. 在主模块中，首先创建配置。
2. 创建Irc和SpeedSensor类的实例。
3. 初始化了irc外围设备的驱动程序。
4. irc外围设备的驱动程序和速度传感器模型在RTOS任务中定期更新。

即用代码写的

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();

就执行而言

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 */

Answer 1

接口

的几个问题

当我查看您正在使用的接口时，有一些问题。您会提到一个FPGA，它假定正在执行正交解码，并维护当前位置的计数器。我想知道为什么它同时支持1X和4X解码，当4X解码更好的时候，但是我注意到getEdgeCounterState()返回一个8位数。这是否意味着FPGA只有一个8位计数器的位置？这看起来很小，为什么不有一个32位的计数器呢?8位足够小，在高转速下，计数器可以在微控制器的读取之间进行封装。所以我要么：

1. 重新编程FPGA，使其有一个更大的计数器，或者
2. 如果您无法更改FPGA，请仔细考虑如何处理计数器包装。

我也对有一个单独的getDirection()函数感到惊讶。如果读取之间的方向发生了迅速的变化呢？或者，如果调用getEdgeCounterState和getDirection()之间的方向发生了变化呢？

有一个用于计时目的的免费运行计数器，但该计数器是否与读出边缘计数器同时读取？否则，计算的速度可能是不正确的。

精度

你打算用速度传感器做什么，它需要有多精确？假设硬件是完美的(例如，OCXO用作时钟源)，由于编码器和代码中所做的数学计算脉冲的离散性质，您仍然有一些不精确的来源。调用update()的频率越高，速度就越不准确，因为时间戳值之间的差异将越来越小，舍入误差也会越来越大。

然后以rad/s的速度转换为float值，该值使用kPi，定义为3.14。真的？这比古埃及人的近似 ( 22/7 )更糟糕。没有理由不为\pi使用更好的价值，比如：

• C++20's std::numbers::pi
• M_PI from <cmath> (可能不适用于所有平台)
• 4 * std::atan(1) (可能无法在constexpr上下文中工作)
• 只需多写一些数字：3.14159265359

脉冲与边缘对正交

/* @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.

这里的文字是不准确的，“脉冲”和“边缘”是以一种草率的方式使用，这将使读者感到困惑。一个编码器有两个输出信号，它们一起构成一个正交输出。两个输出信号中的每一个都会产生脉冲和边缘，但不能准确地说编码器作为一个整体产生“每转X脉冲”。在每一次革命中，最好说“(正交)状态变化”。您已经在注释中提到了状态更改：

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

也许“上升的边缘”确实存在于FPGA的某个地方，但是没有这样的外部可见信号。最好说，“计数”发生在每个有效的状态变化。毕竟，这里有个柜台。

还请注意，这里的注释使4X解码听起来总是被完成，但是您可以将FPGA配置为进行1X解码，因此这意味着每4个有效状态更改就有一个计数。

不将临时值存储在成员变量

中

SpeedSensor.h中有很多成员变量是不必要的。假设您仍然希望使用公共API来计算速度，然后调用update()来计算速度，并调用getRotationalSpeed()来获取当前的速度值，那么您需要在state之后存储的唯一东西是no_edges_previous和time_stamp_previous，它们可以计算位置和时间的差异，以及direction和rotational_speed_rad_per_sec。您可以通过让更多的函数return它们计算的值来实现这一点，而不是将它们存储在成员变量中：

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);
}
...

此外，避免计算和存储的东西，你不使用开始，如转速在RPM。尤其是在嵌入式设备上，RAM是一个有限的资源，所以不要浪费它。

使用volatile struct

我看到您有一个包含struct IrcRegs成员的volatile，但是您有一个不应该是volatile的寄存器的镜像。但是，现在您必须复制一些代码。考虑不要让IrcRegs volatile的成员变得不稳定，而是在需要的地方使整个struct变得不稳定。考虑：

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

volatile IrcRegs *regs;
IrcRegs regs_mirror;

考虑使用std::chrono

如果可能的话，利用C++'s日期和时间实用程序存储时间戳和持续时间。