Embedded programming¶
Introduction¶
I'm very familiar with coding in C++ because of my university. At the moment im studying computer sciences at the Applied University of Amsterdam.
Switching to platformIO.¶
At the local lecture we got thought how to use platformIO. I was already used to Arduino IDE v2. But I found the UI and the speeds of PlatformIO very nice so that's why I decided to switch to PlatformIO.
First off. I created a new project with the Xiao espC3 because the espC6 isn't registered on the list. I've googled a bit and that's because PlatformIO needs to get profit so they ask money to the board developers to add the board to their platform. So we need to set the correct board later on.
When it created the project. I copied all the files over to the src folder and renament my drone.ino to main.cpp. Now I had a lot of include errors because Arduino IDE automatically imports everything and with PlatformIO you need to define everything yourself. So I started out in the Library manager installing all the libraries to the project I needed.
Setting the correct board¶
Before
After
[env:seeed_xiao_esp32c6]
platform = https://github.com/mnowak32/platform-espressif32.git#boards/seeed_xiao_esp32c6
platform_packages =
framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.2
framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.2/esp32-arduino-libs-3.0.2.zip
framework = arduino
board = seeed_xiao_esp32c6
To get the correct configuration if it isn't listed on PlatformIO, is to go to the Board manufactors website and hope there is a PlatformIO example configuration. For example this example was listed on the Xiao website. Link.
Installing libraries in platformIO¶
To install a library click the platformIO icon. In there click the libraries button. Then you can search a library and click add to project. Now you have added a library to your project!
After that it will edit platformio.ini and there you will see the library added to the project. It's also in .pio/libdeps
Important for linux users.¶
When setting up PlatformIO on another system we ran into the issue that the machine couldn't upload the code properly. We first checked how the OS interacted with dmesg. dmesg on linux shows the kernel activity. After a while we found out we needed to install the package platformio-udev. What it does it add some rules for the kernel on how to handle the mcu's.
Programming and compiling and uploading¶
Using platformIO is really straight forward. The only folder you should look at in your platformIO project is the src folder.
In the main.cpp file you can write your program to run on a microcontroller.
On the bottom of your screen you see that there are a few new buttons.
The Compile and Upload and Serial console buttons are going to be the most used. Compile compiles the code and checks if it's correct. If it has incorrect syntax you need to correct it. The upload button compiles the code first and then sends it to the Microcontroller. The Serial console is a really nice debug tool to see what the microcontroller is doing if you added print statements in your code.
First code to read from the BNO085¶
I first wanted to create the drone firmware myself. But then I looked through some research papers and existing programs and saw all the math that was needed to keep it upright. Then I decided to modify an existing program. This was my first attempts at getting the sensor to read
Issues with old code¶
Using the wrong library for the BNO085¶
First used the wrong library I used the Adafruit bno0xx library instead of the Sparkfun bno08x library. The Example script below this reads the BNO085 sensor and returns the values in the arduino serial console.
Failure
#include "conf.h"
#include <Adafruit_BNO08x.h>
#include <sh2.h>
#include <sh2_SensorValue.h>
#include <sh2_err.h>
#include <sh2_hal.h>
#include <sh2_util.h>
#include <shtp.h>
Adafruit_BNO08x bno08x(BNOINTERRUPTSIG);
sh2_SensorValue_t sensorValue;
void setup() {
Serial.begin(9600);
Serial.println("setup started");
// Setup all ESC
// ledcAttach(MOTOR1, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR2, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR3, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR4, PWMFREQ, PWMRESOLUTION);
Serial.print("Setup Started");
}
void loop() {
// put your main code here, to run repeatedly:
sleep(3)
if (!bno08x.begin_I2C()) {
Serial.println("Failed to find BNO08x chip");
sleep(1);
}
Serial.print("Game Rotation Vector - r: ");
Serial.print(sensorValue.un.gameRotationVector.real);
Serial.print(" i: ");
Serial.print(sensorValue.un.gameRotationVector.i);
Serial.print(" j: ");
Serial.print(sensorValue.un.gameRotationVector.j);
Serial.print(" k: ");
Serial.println(sensorValue.un.gameRotationVector.k);
}
//https://randomnerdtutorials.com/esp32-pwm-arduino-ide/
//https://github.com/adafruit/Adafruit_BNO08x/blob/master/examples/rotation_vector/rotation_vector.ino#L25
Example
#include <SparkFun_BNO080_Arduino_Library.h>
#include <Wire.h>
#include "conf.h"
BNO080 myIMU;
void setup() {
Serial.begin(9600);
Serial.println("setup started");
// Setup all ESC
// ledcAttach(MOTOR1, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR2, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR3, PWMFREQ, PWMRESOLUTION);
// ledcAttach(MOTOR4, PWMFREQ, PWMRESOLUTION);
Serial.print("Setup Started");
Wire.begin();
myIMU.begin();
Wire.setClock(400000); //Increase I2C data rate to 400kHz
myIMU.enableRotationVector(50); //Send data update every 50ms}
}
void loop() {
if (myIMU.dataAvailable() == true) {
float roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees
float pitch = (myIMU.getPitch()) * 180.0 / PI; // Convert pitch to degrees
float yaw = (myIMU.getYaw()) * 180.0 / PI; // Convert yaw / heading to degrees
Serial.print(roll, 1);
Serial.print(F(","));
Serial.print(pitch, 1);
Serial.print(F(","));
Serial.print(yaw, 1);
Serial.println();
}
}
void calibrateESC() {
ledcWrite(MOTOR1, 1100);
ledcWrite(MOTOR2, 1100);
ledcWrite(MOTOR3, 1100);
ledcWrite(MOTOR4, 1100);
}
//https://randomnerdtutorials.com/esp32-pwm-arduino-ide/
//https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/blob/main/examples/Example24-UncalibratedGyro/Example24-UncalibratedGyro.ino
New driver¶
After researching for a while and looking through other fab academy projects I found out that other people also made drones with micro controllers and used a pre-made driver that they customized (https://fab.cba.mit.edu/classes/863.23/Architecture/people/Zhixing/finalproject.html). After doing some research on how to keep the drone upright I also decided to use an existing driver because the math required for that is way above my level. Im gonna be using the dRhemFlightVTOL driver. The only problem is that it doesn't support my Inertial measuring unit (BNO085). So I will have to customize the driver to make it work with it.
Editing the new driver¶
Adding in support for the BNO085. The original driver doesn't support the BNO085.
I first started off searching for every instance of an existing sensor.
From there I started out changing everything to BNO085 and also importing the correct library. As I have found the correct library earlier in the code.
The purple text (Compiler flags)¶
The purple text are compiler flags. They tell the compiler what to do with certain pieces of code or variables.
| Flag | What does it do |
|---|---|
| #define |
What define does is it replaces a variable with a value at compilation. This can save some memory by using less variables |
| # if | Everything between #if and #endif or #elif is compiled if the #if is true. So if the variable isn't defined the code will not get included while compiling. So it also won't get run when uploading it to the microcontroller |
| #elif | Same as #if except using this you can have multiple cases |
| #include | Include a library into the code |
| #else | If none of the cases are met this is used |
| #error | Throws an error during compiling when ran by the compiler |
| #endif | end of a #if, #elif or #else |
Setting the correct library¶
I added a compiler case for my sensor and added the correct library to it.
Setting correct value selection¶
I am not sure what it does. It sets some values for the calculations later on. So I copied over the configuration for the MPU6050 because it was the most similar to the BNO085. 
Setting up the sensor¶
For this part I went into the documentation of the Sparkfun BNO0xx library. I first looked through the rest of the code what values it wanted because with the BNO085 you need to enable which information you want.
When digging deeper into the driver to where it pulls the data from the sensors you can see what values it want's and according to that I can go through the documentation to enable the correct data outputs.
I went to the examples list of the library and clicked every data example I needed.
I first clicked the data type I needed.
Then I copied the line that enabled the data output.
Then I inserted it into the driver code.
Getting data from the sensors¶
This step goes similar to the last step except you need to copy over another bit.
What this part does is acquire the sensor data and storing it in a variable. The same needs to be done in the driver like this.
The only issue is that the BNO085 can't output degrees. So we need to convert the radians to degrees using a simple formula I found on google.
The same also has to be done later on in the error correction algorithm
That was the last we needed to change. Hopefully it works now since I still need to fix some compile errors.
Creating my own controls¶
I want to create my own controller. So I need to edit the current controls. The base driver attempts to set up radio communication. We won't be using that so I'm gonna comment that out.
When attempting to compile it still throws some errors so there are still functions being called from the original radio script. The error log indicated it comes from line 1239
So I will be needing to make my own case to control the drone.
Hijacking the controls from the original driver.¶
I need to reverse engineer the controls because now it just calls a function called radiocontrols() that manages the controls. There are 4 pwm channel that controls where the drone is going. I wanna make my own controller and simulate these pwm signals.
thro_des = (channel_1_pwm - 1000.0) / 1000.0; // Between 0 and 1
roll_des = (channel_2_pwm - 1500.0) / 500.0; // Between -1 and 1
pitch_des = (channel_3_pwm - 1500.0) / 500.0; // Between -1 and 1
yaw_des = (channel_4_pwm - 1500.0) / 500.0; // Between -1 and 1
// Triggers for failure criteria
// Line 1260
if (channel_1_pwm > maxVal || channel_1_pwm < minVal)
check1 = 1;
if (channel_2_pwm > maxVal || channel_2_pwm < minVal)
check2 = 1;
if (channel_3_pwm > maxVal || channel_3_pwm < minVal)
check3 = 1;
if (channel_4_pwm > maxVal || channel_4_pwm < minVal)
check4 = 1;
if (channel_5_pwm > maxVal || channel_5_pwm < minVal)
check5 = 1;
if (channel_6_pwm > maxVal || channel_6_pwm < minVal)
check6 = 1;
I wanna control to drone using another microcontroller, specifically an esp because then I can use the espNOW protocol to connect them together and get communication running between them. First I wanna make something functional before I upgrade it so that's why I'm starting out easy.
Components for the controller¶
Im going to start off with 2 sliding potentiometers and a espC3 for throttle and for left-right. The Driver expects PWM frequencies so I'm sending PWM frequencies based on the potsliders.
Wiring the controller¶
To read the Pot meter values we need to read the resistance from the potslider. So we need analog read. When looking at the data sheet I can see that I can use pins from 0 to 5.
Wiring it is pretty straight forward, VCC to 3v3, GND to GND and the out pin to a analog pin.
Programming the controller¶
Testing if I can upload¶
I started out by creating a new PlatformIO project. And selected the Lolin Wemos C3 Mini. Then I tried to upload some code to see if it worked.
In short the answer was yes. I was a bit surprised because the ESP-C3 supermini isn't in the list of platformIO.
Reading the Potentiometer¶
Reading the potentiometer is really straightforward. The only line we need is analogRead(), something to tell the pin that's it an input and something to print it to console.
#include <Arduino.h>
void setup(){
Serial.begin(9600); //Opens serial bus
pinMode(0, INPUT); //sets pin 0 to input
}
void loop(){
Serial.println(analogRead(0)); //prints the result of analogRead() to console
}
When attempting to upload this I got this error a few times.
Then I remembered from the local lecture that if you press and hold boot > press reset > release boot. You can start the microcontroller in boot mode so you can easily flash to it.
After flashing it onto the microcontroller I could test it.
I've noticed there are some ghost values when it needs to be 0 so I need a small area where it's always 0. The upper side of the Potentiometer is always 4095 thats the max value so that's good.
Fixing the ghost measurements¶
Im going to map everything lower than 80 to 80 using software. The reason im doing 80 is because then it will keep on counting up from 80 instead of jumping from 0 to 81 and I didn't see any ghost readings above 80.
Luckily there is an arduino function called map that can do this. Source. The syntax is like this map(value, fromLow, fromHigh, toLow, toHigh) so it's really easy to use. This is how I implemented it.
During coding I realized this could be done way easier and simpler. So this is my result
Code
#include <Arduino.h>
int pot1 = 0;
int Mpot1 = 0; //mapped pot
void setup(){
Serial.begin(9600);
pinMode(0, INPUT);
}
void loop(){
pot1 = analogRead(0); //store potmeter value in pot1
if (pot1 <= 80){ //check if pot1 is lower than 80, if yes make it 80
Mpot1 = 80;
}
else {
Mpot1 = pot1; //if pot1 is above 80 just copy over pot1 to Mpot1
}
Serial.println(Mpot1);
}
This code works and makes any value under 80 automatically 80. And keeps all values above the original value.
I've cleaned it up a bit and turned it into a function so I don't have to copy paste code later on so it stays cleaner. Result:
Code
#include <Arduino.h>
// declarations
int normalizePot(int pin, int minValue);
// constants
const int potPin1 = 0;
void setup()
{
Serial.begin(9600);
pinMode(potPin1, INPUT);
}
void loop()
{
Serial.println(normalizePot(potPin1, 80)); //print the output of the function directly
}
int normalizePot(int pin, int minValue) //function
{
int pot = analogRead(pin); //read the pin
if (pot <= minValue) //if the value is below 80 return 80 and go back to the loop
{
return 80;
}
else //else return the original value and return to the loop function
{
return pot;
}
}
Now I wanna map the potmeter values as if it is a PWM signal. The PWM signal for this script goes from 1100 to 2200 as seen earlier in the documentation. I wanna be more on the safe side and I'm going for 1400 - 2000 range so I don't accidentally trigger the failsafe.
Code
#include <Arduino.h>
// declarations
int normalizePot(int pin, int minValue);
int mapPot(int normalizedValue);
// constants
const int POTPIN1 = 0;
const int MAXPWMVALUE = 1400;
const int MINPWMVALUE = 2000;
void setup()
{
Serial.begin(9600);
pinMode(POTPIN1, INPUT);
}
void loop()
{
Serial.println(mapPot(normalizePot(POTPIN1, 80))); //call normalizePot and put the output into mapPot then print it
}
int mapPot(int normalizedValue){
return map(normalizedValue, 80, 4095, MINPWMVALUE, MAXPWMVALUE); //map the normalized value to the PWM range
}
int normalizePot(int pin, int minValue) //normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate
{
int pot = analogRead(pin);
if (pot <= minValue)
{
return 80;
}
else
{
return pot;
}
}
This is my new code. It maps the values from 80 to 4095 to a range of 1400 to 2000.
Connection between the controller and the flight controller¶
Controller side¶
Now that I have PWM values the driver can understand I wanna test if the driver works and if I can get readings from the driver. Because it needs some kind of controller script to start. For the wireless communication I wanna use ESPNOW, because the protocol works without internet. The 2 esp's directly connect to each other. The only downside is that you're stuck with esp's because the protocol only works on esp's.
I am following this tutorial to create an ESPNOW connection.
What I first need to do according to the tutorial is get the receiver esp their mac adress. Luckily that gets displayed when uploading to the esp.
After MAC there is the mac address. I've saved it in my controller code as a compiler flag #define receiverMAC "d8:3b:da:37:66:00"; So I could use it later on. I don't know if the libraries accepts this compiler flag but I will find out soon enough.
I first need to create a struct with the data I wanna send. A stuct is a collection of variables named under one big variable.
This is a struct in the tutorial but I wanna send my potentiometers their pwm value. So mine will look like this. Now I just realised I was looking at the receiver sketch instead of the sender sketch. But there is no damage done yet because the same struct is needed in both of the scripts. I do need to change the way I declare my mac address. The mac address needs to be stored in an array. like this:// REPLACE WITH YOUR RECEIVER MAC Address
uint8_t broadcastAddress[] = {0xD8, 0x3B, 0xDA, 0x37, 0x66, 0x00};
Code
#include <Arduino.h>
#include <WiFi.h>
#include <esp_now.h>
// declarations
int normalizePot(int pin, int minValue);
int mapPot(int normalizedValue);
// constants
const int POTPIN1 = 0;
const int MAXPWMVALUE = 1400;
const int MINPWMVALUE = 2000;
const uint8_t broadcastAddress[] = {0xD8, 0x3B, 0xDA, 0x37, 0x66, 0x00};
//Define the struct that will be sent
typedef struct struct_message {
int PWMCH1;
int PWMCH2;
int PWMCH3;
int PWMCH4;
} struct_message;
struct_message myData; //declare the struct as myData
esp_now_peer_info_t peerInfo; //create a class object of the ESPNow class
void setup()
{
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
Serial.begin(9600);
pinMode(POTPIN1, INPUT);
}
void loop()
{
Serial.println(mapPot(normalizePot(POTPIN1, 80))); //call normalizePot and put the output into mapPot then print it
// Set values to send
myData.PWMCH1 = mapPot(normalizePot(POTPIN1, 80));
myData.PWMCH2 = 1000; //test values
myData.PWMCH3 = 1000;
myData.PWMCH4 = 1000;
// Send message via ESP-NOW
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
if (result == ESP_OK) {
Serial.println("Sent with success");
}
else {
Serial.println("Error sending the data");
}
}
int mapPot(int normalizedValue){
return map(normalizedValue, 80, 4095, MINPWMVALUE, MAXPWMVALUE); //map the normalized value to the PWM range
}
int normalizePot(int pin, int minValue) //normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate
{
int pot = analogRead(pin);
if (pot <= minValue)
{
return 80;
}
else
{
return pot;
}
}
I added that it could connect to the internet and that it attempts to send data. I quickly realised that the mac address I entered was from the controller itself and not the receiving end. So I quickly corrected that mistake.
Now when I compile it I get errors that the data isn't being send correctly. The issue could be that the second microcontroller isn't turned on with the receiver code.
Drone side¶
For the drone I'm going to start of with a seperate script to see if it works and then start implementing it into the drone driver. I started off with copy pasting the program into my editor and uploading it to my microcontroller. The only thing I needed to do is copy the struct from the controller, because they need to be the same. So this is my code im going to test now.
Code
/*
Rui Santos & Sara Santos - Random Nerd Tutorials
Complete project details at https://RandomNerdTutorials.com/esp-now-esp32-arduino-ide/
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files.
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*/
#include <esp_now.h>
#include <WiFi.h>
// Structure example to receive data
// Must match the sender structure
typedef struct struct_message {
int PWMCH1;
int PWMCH2;
int PWMCH3;
int PWMCH4;
} struct_message;
// Create a struct_message called myData
struct_message myData;
// callback function that will be executed when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
memcpy(&myData, incomingData, sizeof(myData));
Serial.print("Bytes received: ");
Serial.println(len);
Serial.print("Char: ");
Serial.println(myData.PWMCH1);
Serial.print("Int: ");
Serial.println(myData.PWMCH2);
Serial.print("Float: ");
Serial.println(myData.PWMCH3);
Serial.print("Bool: ");
Serial.println(myData.PWMCH4);
Serial.println();
}
void setup() {
// Initialize Serial Monitor
Serial.begin(9600);
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
// Once ESPNow is successfully Init, we will register for recv CB to
// get recv packer info
esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv));
Serial.println("Waiting for data");
}
void loop() {
}
A weird thing I found out is that my esp32C3 Supermini doesn't work wirelessly that well. When I uploaded the code it still didn't work untill I held my finger on the antenna.
I've delved a bit deeper into it and then I found online that with the first batches of espC3 supermini's have issues with the antenna. When I was debugging it I noticed that connecting the ground to the antenna fixed it when I tried it with a pin. But when soldered on it didn't work anymore and then only thing that worked was my finger.
Result of bridging ground and the antenna. Sadly didn't work. After that I grabbed the Xiao espC3 and connected it. It almost worked like a drop in replacement I only needed to change the Potpin. After that it worked flawlessly.
Now I can start integrating the code into the drone driver.
The first thing I did is add the correct libraries at the top of the file. So it looks like this now
#include <Arduino.h>
#include <SparkFun_BNO080_Arduino_Library.h>
#include <PWMServo.h>
#include <esp_now.h>
#include <WiFi.h>
After that I added my own ESPNow define so I could use that later on in the code. I want the code to stay variable and user specified so thats why im continuing the way it was made.
After that I added the global variables for the ESPNow protocol.
Then I uncommented RadioSetup() again so it could be used again and I added a case for my ESPNow protocol.
In the PWM receive function I added an additional case so my PWM values actually get intergrated into the code.
At the end of the program I added the ESPNow Initialization function and the callback function for when the data is received.
When I compile it now it should work. Except that it doesn't work.
From looking at the errors it looked like the PWMServo library wasn't compatible with the ESP32. When looking around on google from PWMservo libraries for the ESP32 I found the correct library on this website. When I added that I needed to edit the class creation to the correct library and then it finally compiles!
From:
To:
Result:
It compiles!
The next day when testing the code. The microcontroller would suddenly disconnect. First I tried setting every pin to something else because there where pins being defined the espC6 doesn't have. When I finally set the debug level to verbose. I did that by adding this line in my platformio.ini file. Source
Now I can see everything that the microcontroller is doing. I saw that it used pin 12 and 13 for USB. And somewhere in the program pin 12 and 13 where re-assigned. So I removed that it got reassigned and then the communication worked flawlessly again.
Now I can finally test. When adding debug print statements for my sensor I found out it wasn't responding. Then I looked at the pins of the sensor and found out when I re-seated it I accidentally killed it.
I had it like this:
Instead of like this.
So I had the 3v3 connected to the sensor it's ground and the ground connected to the SCL pin. So that killed it.
Sources¶
Code¶
- espnow
- arduino map function
- BNO085 library examples
- PlatformIO espC6
- ESC calibration and PWM
- ESP32PWMServo lib
- PlatformIO Debug level change for ESP