Firmware development

The firmware is written in C++ language using Husarion hFramework library. The easiest way to build it is to use Visual Studio Code with Husarion extension.

Preparing the environment

You will need:

• Visual Studio Code A lightweight code editor with rich ecosystem of extensions.

• Husarion extension An extension to VS Code that will help to build and manage applications that use hFramework library.

• (Optional) Git A version control system that will help to track source code changes.

Building Leo firmware

First, you need to clone the leo_firmware repository. If you use the command line, type:

git clone https://github.com/LeoRover/leo_firmware.git

You can also use one of Git GUI clients instead of the command line.

If you don't want to use Git, you can just download the firmware as a ZIP file and extract it somewhere. However, consider that by using Git, it may be easier to merge new changes from repository later on.

Now you need to open the project in Visual Studio Code. Start VS Code, click on File -> Open Folder or type Ctrl+Shift+P, type Open Folder and click enter. Select the leo_firmware folder.

To build it, just type Ctrl+Shift+B.

If the build was successful, leo_firmware.hex file should appear. If the build failed, a terminal should appear with the error message.

Flashing the firmware

To flash the firmware, you can connect USB A <--> microUSB cable between your computer and hSerial port (microUSB port on CORE2), then type Ctrl+Shift+P, Flash Project to CORE2.

You can also upload the leo_firmware.hex file to your Rover and follow this tutorial:

pageFirmware update

Writing your own custom firmware

To write a custom project, create a new folder and open it in VS Code. Then, type Ctrl+Shift+P, Create Husarion project. New files should spawn.

To set project name, open CMakeLists.txt and change myproject to your custom name.

Now, you can build the project (Ctrl+Shift+B) and, if the build was successful, a myproject.hex file should appear (or another if you changed the project name).

Now, you can edit main.cpp file to write your custom code.

There are many examples on Husarion docs page and in hFramework repository that present basic functionality provided by the library. You can also take a look at hFramework API reference to learn more about available classes and their application.

To communicate with Raspberry Pi, however, you need to use ROS topics. The rosserial client library is provided in hROS module, so make sure this line is present in your CMakeLists.txt:

enable_module(hROS)

Here's an example code that uses hFramework to interact with CORE2 board hardware and hROS to communicate with rosserial node on Raspberry Pi:

#include "hFramework.h"

// Include ROS client library and the needed message type definitions
#include "ros.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Empty.h"

using namespace hFramework;

// Declare necessary global variables
ros::NodeHandle nh;

std_msgs::Bool btn_msg;
ros::Publisher *btn_pub;

ros::Subscriber<std_msgs::Empty> *led_sub;

// This function will be called when a new message is received on /toggle_led topic
void toggleLEDCallback(const std_msgs::Empty& msg)
{
    // toggle the led state
    hLED1.toggle();
}

// This function will publish a current button state every 100 ms
void btnLoop()
{
    while (true)
    {
        // Read the button value and replace the message data field
        btn_msg.data = hBtn1.isPressed(); 

        // Publish the message
        btn_pub->publish(&btn_msg);

        //wait 100 ms
        sys.delay(100);
    }
}

void hMain()
{
    // Set baudrate of serial communication to 250kbps
    RPi.setBaudrate(250000);

    // Initialize ROS node with a serial device
    nh.getHardware()->initWithDevice(&RPi);
    nh.initNode();
    
    // Create publisher and subscriber instances
    btn_pub = new ros::Publisher("/btn_state", &btn_msg);
    led_sub = new ros::Subscriber<std_msgs::Empty>("/toggle_led", &toggleLEDCallback);

    // Register new publishers and subscribers
    nh.advertise(*btn_pub);
    nh.subscribe(*led_sub);

    // Create asynchronous task that will publish button state
    sys.taskCreate(&btnLoop);

    while (true)
    {
        // Process all available message callbacks and publications
        nh.spinOnce();

        // wait 10 ms
        sys.delay(10); 
    }
}

Replace main.cpp with this code, build it and flash to your Board.

Now log into your Rover via SSH, and type:

sudo systemctl restart leo

This will restart rosserial node (together with all other ROS nodes). After a while, type:

rostopic list

The topics /btn_state and /toggle_led should appear on the list. The topic /btn_state should be published by the firmware every tenth of a second with a Bool message (True or False) indicating whether hBtn1 on CORE2 is currently pressed. When a message is published to /toggle_led topic, hLED1 should change it's state to opposite.

Try it yourself! Type:

rostopic echo /btn_state

and try pressing the button to see how the value changes.

Then, type repeatedly:

rostopic pub /toggle_led std_msgs/Empty

and notice how the LED turns on and off.

Adding features to Leo firmware

Writing your own firmware for the Rover from scratch can be tedious and disappointing. A more straightforward approach would be to add features to stock leo_firmware.

One of the most commonly desired features is to add GPIO support via ROS topics. In this example we will add one publisher that sends voltage level read at pin2 of hExt port (GPIO input) and one subscriber that sets voltage level (high or low) on pin3 of hExt port (GPIO output).

First, we need to include appropriate message type definitions. For GPIO input, we will use std_msgs/Float32 and for the output std_msgs/Bool, so make sure that after #include "ros.h", these messages are included:

#include "std_msgs/Float32.h"
#include "std_msgs/Bool.h"

Declare these global variables on the top of the file (where other publishers and subscribers are declared):

std_msgs::Float32 gpio_in_msg;
ros::Publisher *gpio_in_pub;
bool publish_gpio_in = false;

ros::Subscriber<std_msgs::Bool> *gpio_out_sub;

Leo firmware publishes all messages in one loop that works with a frequency of 1 kHz. publish_gpio_in will tell this loop that a new message is to be published.

Before initROS() function (where all subscriber callbacks are declared) add this function:

void GPIOOutCallback(const std_msgs::Bool& msg)
{
	hExt.pin3.write(msg.data);
}

Somewhere inside initROS() function, add these lines:

gpio_in_pub = new ros::Publisher("/gpio_in", &gpio_in_msg);
gpio_out_sub = new ros::Subscriber<std_msgs::Bool>("/gpio_out", &GPIOOutCallback);

nh.advertise(*gpio_in_pub);
nh.subscribe(*gpio_out_sub);

Before hMain() function, add:

void GPIOInLoop()
{
	uint32_t t = sys.getRefTime();
	long dt = 100;
	while(true)
	{
		if (!publish_gpio_in)
		{
			gpio_in_msg.data = hExt.pin2.analogReadVoltage();
			publish_gpio_in = true;
		}

		sys.delaySync(t, dt);
	}
}

This function will check, with frequency of 10Hz, if the previous message was published, and if it was, it will read the voltage value and tell the main loop that the next message is to be published.

Now, in hMain() add these lines before initROS() function call to setup the pins on hExt port:

hExt.pin2.enableADC();
hExt.pin3.setOut();

And after initROS(), add:

sys.taskCreate(&GPIOInLoop);

To start the function asynchronously.

The last thing that's left to do is to actually publish the message. To do this, just add these lines to the main loop:

if (publish_gpio_in){
	gpio_in_pub->publish(&gpio_in_msg);
	publish_gpio_in = false;
}

In case you missed something, here's a working example (built on top of 0.5.1 version of leo firmware): https://pastebin.com/7xJRShsS

Build and flash the firmware, log into the terminal and check with rostopic list if the new topics have spawned.

Now, you should be able to check the voltage readings with:

rostopic echo /gpio_in

and set the output level with:

rostopic pub /gpio_out std_msgs/Bool -- "data: true"

true will set the output to high (3.3V) and false will set it to low (0V).