Return to Leonardo Bizzoni's personal page

My work focused on developing the embedded firmware for the Pioneer-3DX mobile robot using an STM32-F767ZI board.
The firmware handles low-level control and sensor reading, including the wheel encoders, and computes the robot’s odometry in real time.
In parallel, I developed a ROS 2 node that communicates with the robot through a custom messaging protocol I designed.
The node publishes odometry information and provides several services to configure the robot, change its execution mode, and trigger emergency procedures.

- Firmware

The firmware for the Pioneer-3DX is based off of the Otto mobile robot firmware but it has been rewritten from C++ to C11 to reduce amguity when reading the code.

After the rewrite i added the odometry calculation to the PID controller loop.
Now odometry and motor control are constantly getting updated at 100kHZ via timer callback, effectively decoupling updates from the communication system.

A key improvement was the creation of a generic FMW layer to separate low-level hardware control from application logic.
This layer provides a simple interface for interacting with motors, encoders, LEDs, and UART callbacks without mixing HAL code with application-specific logic.
For example, functions like the following are part of the FMW layer:

FMW_Result fmw_motors_init(void);
FMW_Result fmw_motors_deinit(void);
FMW_Result fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle);
void fmw_motors_stop(void);
FMW_Result fmw_encoders_init(void);
FMW_Result fmw_encoders_deinit(void);
FMW_Result fmw_encoders_update(void);
FMW_Result fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled, float *linear_velocity);
FMW_Result fmw_encoder_count_reset(FMW_Encoder *encoder);
FMW_Result fmw_encoder_count_get(const FMW_Encoder *encoder, int32_t *ticks);


Using this layer, the application can implement high-level behaviors and message handling without touching low-level HAL details.
This ensures the firmware is easier to maintain, extend, and even port to different platforms that don't rely on the STM32 HAL.

The next step was consolidating initialization.
Instead of writing separate initialization functions for each component, a single fmw_init function was implemented to configure all necessary hardware
and features based on provided arguments.
For example, the Pioneer-3DX initialization looks like this:

FMW_InitInfo fmw_info = {
.motors = motors.values,
.motors_count = ARRLENGTH(motors.values),
.encoders = encoders.values,
.encoders_count = ARRLENGTH(encoders.values),
.emergency = {
.timer = &htim7,
.on_begin = emergency_mode_begin,
.on_end = emergency_mode_end,
.wait_at_most_ms_before_emergency = 2000,
},
.message_exchange = {
.huart = &huart3,
.hcrc = &hcrc,
.handler = message_handler,
},
};
FMW_ASSERT(fmw_init(&fmw_info) == FMW_Result_Ok);


Finally, the firmware includes automatic emergency mode triggering if the ROS2 workstation disconnects or stops sending messages.
During initialization, callbacks can be specified for starting and ending emergency mode, along with a timeout for normal execution without messages.

- ROS2 Node

The ROS2 application is written in C++ and communicates with the STM32 board via the serial interface (/dev/ttyACM0) using the custom messaging protocol.

Its main objective is to retrieve odometry data from the robot and publish it to the ROS2 network, making it available to other nodes in the system. The node also subscribes to velocity commands, which are forwarded to the robot to control its motion in real time.

In addition, the node exposes a set of services to configure and control the robot at runtime. These include PID parameter tuning, robot configuration, LED control, execution mode changes, and emergency stop toggling. This allows external components in the ROS2 ecosystem to interact with the robot in a structured and modular way.

The node acts as a bridge between the embedded firmware and the ROS2 ecosystem, handling message validation, and synchronization between the two systems.

- Communication Protocol

The communication protocol is based on UART and exchanges fixed-size messages between the STM32 and the ROS2 workstation. While UART is currently used, the design is transport-agnostic and can be adapted to other interfaces (e.g., Ethernet) by simply adapting the application logic and extending the firmware code to support the new interace.

Each message has a fixed size of 41 bytes and is fully described by the FMW_Message tagged union. A message is composed of a 5-byte header and a 36-byte body. The header contains the message type (a.k.a. the tag) and a CRC field used for error detection, while the body content depends on the tag. Messages that do not require parameters still include a 36-byte empty body to preserve the fixed-size structure.


typedef struct FMW_Message {
struct FMW_Message_Header {
FMW_MessageType type;
uint32_t crc;
} header;

union {
struct {
float baseline;
float wheel_circumference_left;
float wheel_circumference_right;
uint32_t ticks_per_revolution_left;
uint32_t ticks_per_revolution_right;
} config_robot;

struct {
FMW_PidConstants left;
FMW_PidConstants right;
FMW_PidConstants cross;
} config_pid;

struct {
float voltage_red;
float voltage_orange;
float voltage_hysteresis;
uint32_t update_period;
} config_led;

struct {
float linear;
float angular;
} run_set_velocity;

struct {
int32_t ticks_left;
int32_t ticks_right;
float position_x;
float position_y;
float orientation_x;
float orientation_y;
float velocity_linear;
float velocity_angular;
uint16_t delta_millis;
FMW_Result result;
} response;
};
} FMW_Message;