Mastering motor control: implementation in C++

转载：https://www.embedded.com/mastering-motor-control-implementation-in-c/

Over three decades ago, real-time software was predominantly written in assembly language or a combination of assembly and the C programming language. Even today, certain digital signal processing (DSP) routines are crafted in assembly language and distributed by MCU/DSP manufacturers. This is especially the case when dealing with a specific coprocessor for which compilers are not optimized. Such compilers may not understand the coprocessor’s architecture or how to leverage its advantages, such as efficient memory access, register allocation, execution of specialized instructions, and pipelining to perform multiple instructions in parallel. DSP processors employ these techniques to efficiently execute a series of signal processing operations. A prime example is Finite Impulse Response (FIR) filter. FIR filter code written in assembly language can be up to 20 times faster than a C implementation. However, this is only true for specific coprocessors. Compilers and optimizers for standard MCUs/DSPs are already so efficient that assembly language programming does not significantly outperform the code generated by the compiler. Today, the majority of real-time applications are written in the C programming language.

Programming in C++

C++, especially modern C++ (C++11 and later), incorporates enhanced standard libraries with excellent functions and algorithms, reducing the need for additional third-party libraries. It includes features like Move Semantics and smart pointers like std::unique_ptr, which help reduce the risk of memory leaks. Other features like Type Inference (auto), Range-Based for Loops, a friendlier syntax, excellent performance, a strong ecosystem, and a vibrant community make modern C++ a compelling choice for implementing real-time software.

Despite the numerous advantages mentioned above, there are some issues or challenges that may arise. Here, I aim to highlight these issues and provide guidance on how to address them.

One aspect I would like to emphasize is the use of the ‘new’ and ‘delete’ operators, which involve dynamic memory allocation and deallocation. I’ve already discussed this in the previous article, so I’ll only list issues that haven’t been mentioned thus far:

Exception Handling: The use of exceptions in C++ can lead to non-deterministic behaviours in embedded real time When an exception occurs, the program needs to “unwind the stack,” which involves gathering data needed for function calls and finding the appropriate “catch” block to execute. For this purpose, the program utilizes “unwind tables” created by the compiler. This entire process consumes processor time and is non-deterministic. Hence, it is recommended to use exceptions sparingly or minimally.
Increased Memory Consumption: C++ applications often require more memory than those written in C. If the platform has severely limited memory resources, C may be more suitable than C++. However, we are witnessing constant increases in the resources and performance of modern platforms.
Complexity of C++: Some C++ programmers utilize complex algorithms to solve problems that could sometimes be resolved with just a few lines of traditional C code. Consequently, code readability for other developers may become problematic. These issues can be addressed through the use of coding guidelines and thorough code reviews.
Garbage Collection (e.g., C++’s std::shared_ptr and std::weak_ptr): The use of shared and weak pointers can introduce unpredictable delays. It is recommended to employ deterministic memory allocation strategies, such as std::unique_ptr, which provides automatic memory management without the reference counting overhead associated with std::shared_ptr. It represents exclusive ownership of a dynamically allocated object and ensures that the object is deleted when it is no longer needed, typically when the std::unique_ptr goes out of scope.
Heavy Use of Standard I/O: Input and output operations can be unpredictable in real-time systems. It is advisable to minimize the use of standard I/O functions in critical sections of your code.
Non-Deterministic Timing and Context Switching Overheads of Modern C++: Features like ‘std::thread’, ‘std::mutexes’, and other synchronization primitives like ‘std::unique_lock’, ‘std::lock_guard’, ‘std::semaphore’ etc. of modern C++ are not inherently designed for real-time determinism. While technically possible, using C++ threading features in a real-time application with an RTOS requires careful planning, testing, and consideration to meet real-time requirements. In many real-time scenarios, it may be more practical and reliable to use the native task management and synchronization mechanisms provided by the RTOS, designed specifically for real-time constraints.
Virtual Functions and Polymorphism: Dynamic dispatch through virtual functions can introduce performance overhead. Minimize the use of virtual functions or use them judiciously.
Complex Template Metaprogramming: Keep template usage simple and well-understood.

I would also like to emphasize some interesting features of modern C++ that should be extensively used during application development. Here are a few of them:

Use namespaces: Organizing code using namespaces is a fundamental practice in C++ programming that helps manage the complexity of larger codebases, reduce naming conflicts, and improve code organization. Namespaces provide a way to logically group related code elements, such as classes, functions, and variables, into separate scopes.
Use the std:: prefix when accessing standard library elements.
Using the <algorithm> Library: This library includes a wealth of useful functions such as std::min, std::max, std::sort, std::find, std::copy, std::move, std::adjacent_find, and more.
Using Constants from <limits>: Constants like INT_MAX, INT_MIN, CHAR_BIT, and others can be used to write code that works correctly with specific data types, ensuring that values do not overflow or underflow.
C++ Version of Header Files: Utilize C++-specific header files like <cstdint> to work with fundamental data types (e.g., std::uint32_t).
Data Types and Functions from <cstddef>: Leverage data types and functions like std::size_t, std::ptrdiff_t, nullptr (instead of the old C/C++ NULL), offsetof, alignas, std::byte
Using <array>: Consider using the <array> container for managing arrays.
Using lambdas: Lambdas introduce function objects, which may have a slight performance overhead compared to inline code. In real-time systems, minimizing the use of global functions and variables is often preferred. Lambdas allow you to encapsulate functionality within a limited scope, reducing the potential for naming conflicts

General Real-time Software Development Guidelines

In addition, here are some general programming guidelines:

Efficient Interrupt Handlers: Implement interrupt handlers that execute with minimal time overhead and use platform-specific mechanisms for interrupt management.
Memory Layout and Alignment: Pay close attention to memory layout and data alignment, utilizing “alignas” when necessary.
Compiler-Specific Optimization: Employ compiler-specific optimization flags and techniques to enhance code efficiency without compromising determinism.
Minimize Dynamic Dispatch: Reduce the use of virtual functions and dynamic dispatch to minimize indirection.
Documentation: Write clear and well-documented code to enhance code readability and maintainability.
Fault Detection and Recovery: Design and implement fault detection and recovery mechanisms.
Perform Test-driven Development (TDD)
Code Quality and Compliance: Ensure code quality, safety, and reliability by adhering to relevant industry standards and regulations for real-time systems. Consider using standards like MISRA C++, which provides rules and recommendations for writing safer and more reliable C++ code. In any coding standard, there may be cases where you need to deviate from specific rules due to exceptional circumstances or technical reasons. There can be situations where strict adherence to a particular rule may not be feasible or appropriate. In such cases, it is important to define exceptions to the rules with explanations for why the rule cannot be followed as-is.
Static Analysis and Code Reviews: Utilize static analysis tools to identify potential issues in your code and conduct regular code reviews to catch problems early and ensure adherence to coding standards.
Testing Strategies: Implement testing strategies, unit testing, integration testing, and system-level testing, to validate real-time behaviour. Use technique like Software-in-the-Loop (SIL) to validate and verify the functionality of the software in a simulated or emulated environment on the host. Consider conducting Hardware-in-the-Loop (HIL) testing if your project’s financial resources and available infrastructure permit it, especially in safety-critical applications where such testing is typically mandated.

Test-Driven Development (TDD) of Motor Control Application

Here, I will present the Test-Driven Development approach for a motor controller application using a user-friendly development environment, specifically on a Windows PC with Microsoft Visual Studio and Google Test.

The components of this application will later be compiled with the hardware manufacturer’s SDK. The objective is to iteratively develop and test the majority of the application on the PC, using unit tests based on Google Test for testing a significant portion of the code. Some parts, such as the PMSM (Permanent Magnet Synchronous Motor) and certain hardware components, will be simulated.

The primary aim is to facilitate code refactoring and optimization while maintaining comprehensive test coverage and automating the testing process. TDD with PC-based unit tests can significantly enhance code quality, ease of maintenance, and overall reliability for real-time embedded applications. It allows for early detection of defects during the development phase and ensures that the code performs as expected, which is especially important in safety-critical systems.

PMSM Simulator

click for full size image

Figure 1: Mathematical Model of PMSM in dq space after linearization

click for full size image

Figure 2: Synchronous Motor Operational Model

My C++ open-source library ToolboxR, which is in the public domain (MIT License), offers a broad range of essential basic time-discrete blocks for discrete-time modelling. It also includes advanced components such as Finite Impulse Response (FIR) filters, various models, PID controllers, and trajectory generation tools. The parts of the library used in this article serve as examples. You can find the complete library on GitHub at the following link: https://github.com/borisRadonic/ToolboxR

The PMSM Simulator, implemented as a C++ class called PMSMotor employs four discrete-time Forward Euler integrators. The main process function is depicted on Figure 3. The entire linearized PMSM simulator in the DQ space, which will allow us to test the motor controller’s implementation, is achieved with just a few lines of code.

Figure 3: Process function of PMSMotor

Other discrete time models which are interesting for motor control applications provided in ToolboxR library are:

Difference: The Difference class outputs the current input value minus the previous input value.
Time Derivative: The Discrete Derivative approximates the derivative of its input.
FIR Filter: This class implements a discrete-time finite impulse response (FIR) filter.
First-Order IIR Filter: The IIR Filter class implements a discrete first-order infinite impulse response (IIR) filter on the specified input signal.
Discrete Second-Order IIR Filter: The IIR Filter class implements a discrete second-order infinite impulse response (IIR) filter on the specified input signal.
Discrete IIR Filter: The IIR Filter class implements a discrete-time infinite impulse response (IIR) filter on the specified input signal.
Discrete Integrator: Discrete Integrator class performs discrete-time integration or accumulation of signal. It implements Trapezoidal, Forward Euler and Backward Euler integration methods.
Discrete PID Controller: The Discrete PID Controller class implements a PID controller (PID, PI, PD only or I only). It includes Output saturation limits and anti-windup mechanism.
Wave Form Tracer: This utility class can be used to trace selected signals in Gnuplot format files. However, it’s important to note that the Waveform Tracer class is not designed to run on real-time platforms.

Figure 4: Process function of Discrete Integrator

The main ‘process’ function of the discrete integrator class is shown above, supporting various integration methods including Forward Euler, Backward Euler, and Trapezoidal. These methods are commonly employed for numerical integration, an essential component in the simulation of dynamic systems.

Controller

click for full size image

Figure 5: Bridging Real Hardware and PMSM Simulator in Controller Testing

The main controller (named as the ‘Current Controller’) that will be used for both simulated and real hardware is shown in Figure 5. The image shows what in the controller test replaces the PMSM simulator and the relationships and boundaries between software and hardware components. The image also displays some of the RToolbox C++ classes (marked in red) that will be utilized for the purposes of this article. ‘PMSMPICurrentController’ is the fundamental FOC ‘current’ controller, which was mentioned and partially explained in the first article. It is implemented using basic building blocks of the RToolbox library, such as PIDController, Derivative element, IIRFirstOrderFilter, and others. For illustrative purposes in this article, the Velocity controller is implemented within the ‘TestPMSM’ test. Using the WaveFormTracer class, the test result is the ‘TestPMSM.dat’ file, which can be displayed using GNUPlot or python script.

The “PMSMPICurrentController” class is a comprehensive control system designed to manage the operation of Permanent Magnet Synchronous Motors, both in real hardware applications and in simulations. This class encapsulates all necessary control and signal processing parameters essential for effective motor management. The class sets parameters crucial for the operation of the PMSM PI (Proportional-Integral) Current Controller. It is adept at handling both the control dynamics of the motor and the intricacies of signal processing.

Filter Coefficients Configuration: The class allows the setting of coefficients for IIR (Infinite Impulse Response) filters. These filters are used as pre-filters for PID motor control.

PI Controller Parameters: The class configures the proportional (Kp), integral (Ki), and, uniquely, derivative (Kd) gain coefficients for both the q-side and d-side controllers. Although derivative gains are not typical in PI controllers, their inclusion provides comprehensive control capabilities. Additionally, anti-windup gains (Kb_q and Kb_d) and output saturation limits (UpSat_q and UpSat_d) for both controllers can be set, ensuring robust performance under varying operational conditions.

Motor-Specific Properties: Parameters like l_d and l_q, representing the inductance of the stator in the dq frame, Kemf for the back EMF constant, and motor_pole_pairs for defining the number of motor pole pairs, are essential in tailoring the controller to the specific motor in use.

Sampling Period: The parameter ‘ts’ sets the sampling period, crucial for determining the rate at which the controller processes and responds to motor feedback.

Space Vector Pulse Width Modulation (SVPWM) module, as shown in Figure 5, must be implemented for efficient motor control. It’s a sophisticated scheme used with Field-Oriented Control (FOC) to optimize DC bus voltage utilization in inverters, reduce harmonics, and enhance torque. While there’s extensive literature and many software examples on SVPWM, this article focuses on its essential roles in inverter control and motor efficiency, rather than delving into technical details.

Velocity computation can be effectively achieved by implementing the first derivative of position using a Discrete Derivative element, combined with a first-order IIR filter for signal smoothing. However, it’s important to note that every filter, including the IIR filter, introduces a phase delay. Such delays, particularly in feedback signals, are generally undesirable and should be carefully managed in the system design. Another sophisticated method for estimating velocity in control systems, especially when you have access to position data and a model of the system, is through the implementation of a Kalman filter. Implementing a Kalman filter for velocity estimation using position data and a system model can be an effective approach, particularly in scenarios where accuracy, responsiveness to change, and handling of noisy data are critical. Using high-resolution position sensors makes the implementation of a Kalman filter for velocity estimation unnecessary.

Figure 6: Process function of the PMSMPICurrentController

Position Controller

click for full size image

Figure 7: PMSM Position Controller

Figure 7. Shows cascade control structure with feedforward for PMSM drive.

Trajectory Generator

The Trajectory Generator is responsible for planning the path that the motor should follow to reach a desired position.

The Trajectory Generator works in tandem with the Feed Forward control system. Feed Forward control, is inherently predictive, utilizing the principles of physics, particularly mechanics and motion, to anticipate system responses. It reduces a control effort and allows the P feedback controller to focus primarily on compensating for unpredictable disturbances and errors, while the Trajectory Generator ensures that the motor moves smoothly and accurately along a predefined path.

The ToolboxR Library comes with an advanced array of Trajectory Generation Tools, providing top-tier precision and effectiveness in motion planning. These tools are expertly tailored to suit diverse applications, simplifying the creation and handling of intricate trajectories. Ideal for use in robotics, simulations, or any system needing precise motion control. The “Jerk Limited Trajectory” feature in ToolboxR leverages a novel algorithm incorporating Bézier functions, offering advanced control and precision in motion planning. Users can input initial and final positions, velocities, and accelerations, ensuring the trajectory adheres seamlessly to these parameters. Limiting jerk (the rate of change of acceleration) results in smoother transitions in acceleration and deceleration and reduces the mechanical stress on the components of a system.

Additionally, ToolboxR includes a “Quintic Polynomial Trajectory” feature, enabling the generation of trajectories based on quintic polynomials. This tool provides options for setting limits on acceleration and velocity, offering flexible and precise motion planning.

Furthermore, the “Simple Heptic Polynomial Trajectory” tool uses heptic polynomials for trajectory generation. This feature offers similar constraints as the quintic polynomial tool, allowing for detailed and controlled trajectory planning across various applications.

The Position Control Example TEST(TestCasePMSM, TestPMSMPosController) is found here: https://github.com/borisRadonic/ToolboxR/blob/master/CntrlLibraryTest/TestPMSM.cpp

click for full size image

Figure 8: PMSM Position Controller Simulation

The PMSM Position Controller is equipped with a Notch filter designed to suppress frequencies that cause resonant vibrations. Resonance in motors and control systems, characterized by certain frequencies inducing excessive movement or vibration, can significantly hinder performance in precision-oriented applications such as CNC machines or robotics. By mitigating these resonant vibrations, the Notch filter contributes to enhanced accuracy and consistency in operation.

For optimal performance of the Notch filter, it’s crucial to determine its parameters based on the system’s specific resonant frequencies. This necessitates conducting detailed frequency response measurements directly on the system, ensuring that the filter is precisely tuned to counteract the identified problematic frequencies effectively.

Resonances in mechanical systems can arise from various factors such as mechanical imbalances in rotating parts or external sources of vibration. Predominant causes include structural inadequacies, like insufficient stiffness or poorly designed joints, the inherent natural frequencies of mechanical components, and control systems that are not optimally designed. To mitigate the risk of resonances caused by control system design, it’s crucial to isolate specific parts of the control system during frequency response testing. An effective approach to measure the system’s frequency response involves activating only the torque controller. The input in this scenario should be a sinusoidal torque signal spanning a range of frequencies that cover the entire spectrum of interest. This method ensures a focused and accurate assessment of how the system responds to varying frequency inputs, specifically in the context of torque control.

Executing frequency response measurements on the actual system is crucial during the stages of system identification and controller validation. This direct measurement approach is essential for precisely evaluating the system’s performance and for verifying the effectiveness of the control strategy.

M&D