# 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 before deciding to using existing code
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](https://github.com/nickrehm/dRehmFlight/tree/master) 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.
```cpp
  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. 

```cpp
  // 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](https://ardupilot.org/copter/docs/esc-calibration.html)

#### Attaching the esc's to the correct pin.

