This repository contains the Embedded firmware code for the STM32H7 board (NUCLEO-H743ZI) on the Kugle robot developed as part of the master thesis work described in Kugle - Modelling and Control of a Ball-balancing Robot.
The embedded firmware includes all the necessary low-level drivers and high-level application libraries such as the Sliding mode balance controller, Quaternion extended Kalman filter (QEKF) for quaternion estimation and other estimators. To a certain extent the firmware is based based on code generated by MATLAB Coder with the scripts in Kugle-MATLAB.
The Kugle V1 prototype includes a new and fast microprocessor platform for which several low-level drivers are developed to connect the microprocessor to all the external periphirals on the robot. FreeRTOS is used on the microprocessor to enable a multi-threaded processing environment. All code for the microprocessor is implemented as C++ libraries for readability, traceability and ease of debugging, and can be found in Libraries.
The complete firmware stack shown in the figure below, has been developed from scratch, including all low-level hardware libraries, periphiral libraries, drivers, all the way up to the application layers. The choice of a multithreaded object-oriented working environment will hopefully allow future users to quickly expand the features of the embedded firmware without affecting existing applications already running.
The bottom of the firmware stack, also known as the hardware layer, HAL, includes all microprocessor-specific libraries with individual classes for each peripheral.
The device drivers includes all the necessary drivers for the onboard devices such as the IMU, motor drivers, batteries and the package protocol, LSPC, used for the USB communication. LSPC, short for Lightweight Serial Package Communication, is a simple protocol for serial stream interfaces enabling message passing with packages tagged with an ID. The LSPC library thus gives the embedded firmware a publish/subscribe mechanism for passing messages to and from the onboard computer. The list of LSPC message types and IDs are defined in the struct at Libraries/Devices/LSPC/MessageTypes.h.
Both the IMU and LSPC library is templated to allow several interface options. As an example the MPU-9250 supports both I2C and SPI communication, which has been made completely transparent with the MPU-9250 library. In general, device drivers are the only libraries with direct dependency to periphiral libraries.
The module libraries includes self-contained units which may or may not depend on device drivers.
The controller and estimator libraries are all independent classes who only perform mathematical operations and possibly include some time dependent states. Some of these libraries are generated with MATLAB Coder, such as the QEKF, VEKF and model matrices.
The power management library monitors the batteries.
The parameters library includes a global list of dynamically modifiable parameters, thereby serving the purpose as a global parameter server. The global parameter server is linked to an LSPC object so that the parameters can be modified from the onboard computer, see Reconfigure GUI.
The debug library implements an overload on the printf function for passing printf messages directly to the onboard computer over USB, using a debug package, packed with LSPC.
Some miscellaneous support libraries are developed to make mathematical operations such as matrix multiplication and quaternion operations easier. These math libraries are developed as plain C functions rather than classes, so that they can be called everywhere.
Finally, the application layer includes the main applications of the firmware. The characteristic of an application is a thread with a main loop that can be started, stopped or restarted. An application is thus very similar to a service in a Unix system. Two main applications are developed in the embedded firmware, namely the balance controller and the front panel.
The balance controller contain all the necessary logic and flow to run the controller and estimators and set the torque output accordingly, see Balance controller. The front panel updates the user LEDs and monitors the buttons, e.g., handling the shutdown button which can be configured to start and stop the controller.
All libraries are developed with attention to thread-safety and thus include semaphores, mutexes, queues etc. where necessary. As an example the library for the I2C periphiral, being a bus-standard that supports multiple devices through addressing, is developed in such a way that multiple objects pointing to the same I2C periphiral, can be constructed and used independently across multiple threads.
Some of the features within the embedded firmware libraries include:
- Object oriented (C++) thread-safe Low-level periprhiral libraries
- Xsens IMU driver
- MPU9250 IMU driver
- ESCON motor driver
- Quaternion math library
- Quaternion estimator
- Madgwick
- Kinematics library
- Velocity estimator
- LQR controller
- Sliding mode controller
- Heading independent mode
- Velocity LQR controller
- Drag detection (when the robot is being dragged, thus changing the position reference)
- Torque ramp up when starting
- Lift detection (motor failure detector can be used for lift detection by looking at the feedback from the ESCON drivers)
- Wheel Slip detection
- Multi-threaded Publish/subscribe message system over USB to embedded firmware, utilizing LSPC, with callback function registration - see API Protocol.pdf.
- ROS driver publishing estimates, raw sensor topics etc. and subscribing to reference topics
- ROS reconfigurable parameters for easy tuning
- MATLAB dump functionality - logging of everything (raw measurements, estimates, covariances, control outputs etc.)
- printf functionality working over LSPC
- Bootloader functionality for updating the firmware over USB
main.cincludes themainfunction which configures the microprocessor clock and the FreeRTOS kernel. Only one task,MainTask, is created and started with the FreeRTOS operating system.MainTask.cppincludes the code for theMainTaskwhich is started as the first thing when the MCU boots. The sole purpose of this task is to create object instances of the different libraries and link them together with the different modules of the system. Since some of the modules creates other tasks, this process of creating all the object instances is what starts the whole system. After creating and linking the objects theMainTaskthread goes into a monitoring mode, keeping track of the MCU load.Librariesincludes sub-folders for all the libraries listed in the firmware structure drawing above.Libraries/Periphiralsincludes all custom periphiral drivers, e.g., PWM, Encoder, and USBCDC.Libraries/Devicesincludes the individual devices drivers, e.g., ESCON, MPU9250, MTI200 and LSPC.Libraries/Modules/Controllersincludes all the controller libraries, e.g., the Sliding mode balance controller and the Velocity LQR controller.Libraries/Modules/Estimatorsincludes all the estimator libraries, e.g., the QEKF and VEKF.Libraries/Modules/Parametersincludes the code for the parameter library including the header file,Parameters.h, where all parameters are defined.Libraries/Applicationsincludes all the application tasks, e.g., the Balance controller and Front panel handler.
The firmware includes both a balance controller and a velocity controller as described in Controller structure.
The main part of the firmware that ties everything together is the control loop inside the balance controller. The balance controller loop runs at 200 Hz and implements a sequence of tasks shown below. The flow of the balance controller loop is depicted in six steps where the actions within each step are executed from top to bottom. The coloured boxes indicate the main library used for each action.
The first step is to collect measurements from the different sensors. These sensor values are used in the second step for state estimation.
In the third step the control mode is defined with a dynamic parameter. The control mode can be set to either balance control, where just the balance controller is running, or set to velocity control, where a cascaded configuration of the balance controller and velocity LQR controller. Note that in velocity control mode the velocity controller runs in the same loop just prior to computing the sliding mode control.
Different type of references for the controllers can be sent over USB through LSPC and are automatically parsed in the control loop.
The fourth step involves computing the control. The depicted actions illustrate the sliding mode balance controller which consists of first computed an equivalent control part following by a switching part.
Finally, the computed control torque is sent to the ESCON motor drivers and a torque feedback is used to detect motor failures and lift.
As a last step several debug informations are queued for transmission to the onboard computer for ROS topic transmission and logging in general, including a large float array with a raw dump of everything to be processed in MATLAB.
The exact sequence of steps is listed below.
- Refresh parameters from internal parameter "server"
- Get measurements from IMU (polling mode)
- Correct measurements based on IMU calibration (accelerometer bias, accelerometer scale, gyroscope bias, rotation alignment/COM alignment)
- Read encoder values - utilizing processor timer capability of quadrature decoding
- Send raw sensor values to PC
- Estimate orientation using Madgwick or QEKF
- Compute kinematics-based velocity
- Integrate kinematics-based velocity to get dead-reckoning-based position estimate (odometry)
- Estimate velocity using Velocity EKF
- Send estimates to PC
- Extract/compute references based on control mode and setpoints (Quaternion control vs Velocity control)
- Run wheel slip detector on new encoder values
- Run controller (LQR or Sliding mode, depending on controller selection)
- Saturate torque output based on configurable limits
- Perform torque ramp up step, if the controller was just started
- Set motor output torque
- Read motor feedback torque
- Run motor failure detector on output torque and feedback torque (for lift detection)
- Send controller info, debug and statistics to PC
- Send MATLAB dump messages
- When creating objects of classes of larger size, use "new" to allocate the memory for the object in heap instead of on the stack
- Example of how to do this with the object as a pointer:
MPU9250 * imu = new MPU9250(spi); - Example of how to do this while keeping the object as a reference (instead of a pointer) is:
MPU9250& imu = *(new MPU9250(spi));
- Example of how to do this with the object as a pointer:
System Workbench, an IDE based on Eclipse, has been used as the development environment for this embedded firmware. System Workbench is a cross-platform compatible IDE based on Eclipse bundled with both libraries for STM32 devices, GDB debugger and ST-Link debugging capability, enabling easy programming and debugging of embedded applications for STM32 devices.
The IDE can be downloaded from http://www.openstm32.org
If you get an error similar to Could not determine GDB version using command then you might have installed the 64-bit version of System Workbench. However the bundled GDB debugger is a 32-bit version based on the Linaro releases, why you will have to install the corresponding 32-bit version of ncurses.
sudo apt-get install lib32ncurses5Activative the DFU bootloader through the firmware by calling the Enter_DFU_Bootloader function.
Now a new firmware can be loaded into the board using the dfu-util tool from http://dfu-util.sourceforge.net/dfuse.html
./dfu-util -a 0 -s 0x08000000:leave -D KugleFirmware.binThe current firmware (reading the whole flash) can also be retrieved from the board using
./dfu-util -a 0 -s 0x08000000:2097152 -U read.binOBS. If you receive a permission error you can either run the command as root (sudo ...) or you can install the udev rule.
To reset the board out of DFU the dfu-tool built in to Linux can be used
./ResetDFUfromCommandline.shInstead of using the IDE for programming the firmware, the flash can be erased and programmed through terminal.
The flash can be erased by using the EraseFlashFromCommandline.sh script after modifying the OPENOCD_PATH and OPENOCD_SCRIPTS_PATH accordingly.
./EraseFlashFromCommandline.shThe currently running firmware can be restarted through the onboard programmer by calling:
./ResetFromCommandline.sh