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.

Using PlaformIO

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.