Drone-FabAcademy-2025/docs/Assignments/week_4_programming/programming.md

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.

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

```cpp
#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

```cpp
#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.

Implementing the BNO085

Hijacking the controls

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

From looking further in the code it has safety protocols when the channels go too low or too high so using that we have a range where the pwm speed should be.

  // 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;

In this case minVal is 800 an maxVal is 2200. So we need to stay between these ranges to control the drone. In theory you can send any value to the ESC because it calibrates to the lowest and highest values you give it but there is a range. From past experiences I have experienced that 12bit PWM is not sufficient, 16 bit is needed.

ESC calibration

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

[env:seeed_xiao_esp32c3]
platform = espressif32
board = seeed_xiao_esp32c3
framework = arduino

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.

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.

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 it is compiled
#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.

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.

• PotSlider x2
• espC3 supermini x1

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 datasheet 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.