Custom Flight Controller using Arduino Uno

Hi,

I tried to build my own custom flight controller using Arduino Uno.
You can find all the source code at my github repo: https://github.com/anilkunchalaece/arduQuad

Following are the few test flights i tried :

I also developed my own GUI using Python (PyQt4 and PyQt Graph) for debugging and tuning

This is the one of the many problems i had to tackle to get above flight performance

The problem in the above video is using only Angle PID loop. to get minimum takeoff we have to use cascaded PID loops i.e Angular Rate followed by Angle PID loop.

The code i used to achieve take off is https://github.com/anilkunchalaece/arduQuad/blob/master/ArduinoCode/ksrmQuadLevelModePlusConfig/ksrmQuadLevelModePlusConfig.ino

Source code for python GUI
https://github.com/anilkunchalaece/arduQuad/blob/master/PythonCode/GUI/graphGUI_v2.py

Following are the few references i used

https://github.com/benripley/Arduino-Quadcopter

https://github.com/lobodol/drone

https://github.com/metbosch/Arduino-Quadcopter

http://www.brokking.net/ymfc-al_main.html

https://blog.owenson.me/build-your-own-quadcopter-flight-controller/

This is me flying APM2.7 with the same build.

I suck at flying, still lot to learn.
This is how it ended up, crashing on a tree.

and luckily nothing broken :)


Btw i used complementary filter to get the angles from MPU 6050. Drop a comment if are doing the same, i am very happy to join with you to continue this.

Reading Accelerometer Angles

/*
 * Author : kunchala Anil
 * Email : anilkunchalaece@gmail.com
 * Date : 22 May 2018
 * 
 * This sketch is used to calculate angle from MPU6050 Accelerometer values
 * crust of the sketch is 
 *        float pitch = atan(accX/sqrt(pow(accY,2) + pow(accZ,2)));
 *         float roll = atan(accY/sqrt(pow(accX,2) + pow(accZ,2)));
 * 
 * Check following https://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf
 * i didnt understand the math behind it
 */
 
#include <Wire.h>
#include <math.h>

const long ACCR_SENSITIVITY_SCALE_FACTOR = 16384.0; // for 2g
const int MPU6050_ADDR = 0b1101000;

const byte PWR_REG_ADDR = 0x6B;

const byte ACCR_CONFIG_REG_ADDR = 0x1C;
const byte ACCR_READ_START_ADDR = 0x3B;

int16_t accX,accY,accZ;
double angleX,angleY; 
double aX,aY,aZ;

void setup() {
 Wire.begin();
 Serial.begin(115200);
 configureMPU();
}//end of setup

void loop() {
  readAccrX();
// apply trigonometry to get the pitch and roll:
float pitch = atan(accX/sqrt(pow(accY,2) + pow(accZ,2)));
float roll = atan(accY/sqrt(pow(accX,2) + pow(accZ,2)));
//convert radians into degrees
pitch = pitch * (180.0/PI);
roll = roll * (180.0/PI);
  

  Serial.print(pitch);
  Serial.print("   ");
  Serial.println(roll);
  delay(100);
}//end of loop

void readAccrX(){
   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(ACCR_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,6,true);  
  accX=Wire.read()<<8|Wire.read(); 
  accY=Wire.read()<<8|Wire.read(); 
  accZ=Wire.read()<<8|Wire.read(); 
//  Serial.println(accX);   
//  aX = accX / ACCR_SENSITIVITY_SCALE_FACTOR;
//  aY = accY / ACCR_SENSITIVITY_SCALE_FACTOR;
//  aZ = accZ / ACCR_SENSITIVITY_SCALE_FACTOR;

}//end of readGyroX Fcn

void configureMPU(){
  //Power Register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_REG_ADDR);//Access the power register
  Wire.write(0b00000000);
  Wire.endTransmission();

  //Gyro Config
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(ACCR_CONFIG_REG_ADDR);
  Wire.write(0b00000000);
  Wire.endTransmission();
}//end of setUpMPU Fcn

MPU6050 - Calculating the Angle from Gyroscope Angular Rate

/*
 * Author : kunchala Anil
 * Email : anilkunchalaece@gmail.com
 * Date : 21 May 2018
 * 
 * This sketch is used to calculate angle from MPU6050 GyroScope Angular Rate
 * Ingridient of sketch is
 *     Angle = prevAngle + presentAngularRate * dt
 *     prevAngle = Angle
 *     
 *     dt -> time elapsed from the last angular rate reading
 *     
 *     Offset Value is used to slow the Gyro Drift
 *     
 *  For discussion on the code ref : http://forum.arduino.cc/index.php?topic=548675
 */
#include <Wire.h>
const float GYRO_SENSITIVITY_SCALE_FACTOR = 131.0; //for 200 degrees / sec
const int MPU6050_ADDR = 0b1101000;
const byte PWR_REG_ADDR = 0x6B;
const byte GYRO_CONFIG_REG_ADDR = 0x1B;
const byte GYRO_READ_START_ADDR = 0x43;
int16_t gyroX;
float rotX; // It is Angular Rate
float prevRotX=0;
unsigned long prevTime=0,currentTime;

float offsetVal;
const int noOfSamplesForOffset = 1000;

void setup() {
 Wire.begin();
 Serial.begin(115200);
 configureMPU();

  float totalVal = 0;
  for (int i=0; i< noOfSamplesForOffset; i++){
    readGyroX();
    Serial.println(rotX);
    totalVal = totalVal + (rotX);
  }//end of for
  Serial.println(totalVal);
 offsetVal = totalVal / (noOfSamplesForOffset);
 Serial.print("offset Val");
 Serial.println(offsetVal);
 //while(1);
}//end of setup

void loop() {
   
  if(prevTime == 0){
    readGyroX();
    prevTime = millis(); 
  }else{
    readGyroX();
   
    currentTime = millis();
    double dt = (currentTime - prevTime)/1000.0;
    prevTime = currentTime;
    rotX =  prevRotX + (rotX-offsetVal)* dt;
    prevRotX = rotX;
    Serial.println(rotX);
  }//end of ifElse
  delay(100);
}//end of loop

void readGyroX(){
   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,2,true);  
  gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn

void configureMPU(){
  //Power Register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_REG_ADDR);//Access the power register
  Wire.write(0b00000000);
  Wire.endTransmission();

  //Gyro Config
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_CONFIG_REG_ADDR);
  Wire.write(0b00000000);
  Wire.endTransmission();
}//end of setUpMPU Fcn

Checking Basic Lift - using Single Channels

Checking all connections in Flight Controller board "ok" on board using simple Sketch

Channel 3 (Throttle) is used for checking the functionality.

/*
  Author : Kunchala Anil
  email : anilkunchalaece@gmail.com
  Date : 14 Mar 2018

  Checking the altitude of KSRM Quad


*/
/*
   Connect Channel 3 to pin 10 i.e PCINT2
   Arm the all the Motors
   use Servo Write to Send the control signal to ESC

        To Arm ESC use value 60
        Max Speed Can be Achieved using 130

        Pulse Width when Throttle at Minimum position is : 1150
        Pulse width when Throttle at Maximum Posistion is : 1800

        we need to map value from 1200-1800 to 60 to 130
*/


//PCMSKx - Pin Change Mask Register
//PCMSK0 - portB (D8-D13) (PCINT0  - PCINT6)
//PCMSK1 - portC (A0-A5)  (PCINT8  - PCINT14)
//PCMSK2 - portD (D0-D7)  (PCINT16 - PCINT23)
//PCICR -  Pin Change Interrupt Control Register
//  PCIE0 - Pin Change Interrupt Enable 0 - Port B (D8-D13)
//  PCIE1 - Pin Change Interrupt Enable 1 - Port C (A0-A5)
//  PCIE2 - Pin Change Interrupt Enable 2 - Port D (D0-D7)

#include<Servo.h>

#define rxPin 10
#define DEBUG

//MotorConnections

#define topLeftMotorPin 4
#define topRightMotorPin 5
#define bottomLeftMotorPin 6
#define bottomRightMotorPin 7

#define motorArmValue 60
#define motorMinValue 65
#define motorMaxValue 130

#define throttleMinValue 1200
#define throttleMaxValue 1800

volatile boolean recvPCInt = false;
volatile byte portValue;
unsigned long pwmDuration;
unsigned long pwmStart;
unsigned long pwmEnd;

Servo topLeftMotor;
Servo topRightMotor;
Servo bottomLeftMotor;
Servo bottomRightMotor;

//Interrupt Service Routine will fire when for PinChange in PortB
ISR(PCINT0_vect) {
  recvPCInt = true;
  portValue = PINB; //PINB is used to read all port input values
}

void updateMotors(int val) {
  topLeftMotor.write(val);
  topRightMotor.write(val);
  bottomLeftMotor.write(val);
  bottomRightMotor.write(val);
#ifdef DEBUG
  Serial.print("motors are updated with value => ");
  Serial.println(val);
#endif
}


void initializeMotors() {
  topLeftMotor.attach(topLeftMotorPin);
  topRightMotor.attach(topRightMotorPin);
  bottomLeftMotor.attach(bottomLeftMotorPin);
  bottomRightMotor.attach(bottomRightMotorPin);
#ifdef DEBUG
  Serial.println("motors initialized");
#endif
}//end of initializeMotorsFunction


void armAllMotors() {
  topLeftMotor.write(motorArmValue);
  topRightMotor.write(motorArmValue);
  bottomLeftMotor.write(motorArmValue);
  bottomRightMotor.write(motorArmValue);
#ifdef DEBUG
  Serial.println("motors are Armed ");
#endif
}//end of armAllMotors function



void setup() {
  cli(); //Clear all interrupts
  PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13
  PCMSK0 |= 1 << PCINT2;// Pin9
  sei(); //enable all interrupts
  pinMode(rxPin, INPUT);
  digitalWrite(rxPin, HIGH); //enable pull up in pin
#ifdef DEBUG
  Serial.begin(9600);
#endif
  initializeMotors();
  armAllMotors();
}//end of setup

void loop() {
  if (recvPCInt == true) {
    boolean event = digitalRead(rxPin);
    if (event == HIGH)
    {
      pwmStart = micros();
    }
    else if (event == LOW) {
      pwmEnd = micros();
      int width = pwmEnd - pwmStart;

      int mappedValue = map(width, throttleMinValue, throttleMaxValue, motorMinValue, motorMaxValue);

       updateMotors(mappedValue);

#ifdef DEBUG
      Serial.print("Throttle Value is  ");
      Serial.println(pwmEnd - pwmStart);
#endif
    }
    recvPCInt = false;
  }//end of If

}//end of loop

Multi Channel Receiver Signals using Pin Change Interrupts - Reading Four Channels Simultaneously

The main Logic is :
1.Calculate PWM Starting Value during Raising Edge
 2.When Falling Edge Detected Calculate Width

/*
   Author : Kunchala Anil
   Email : anilkunchlaece@gmail.com
   Date : 6 Mar 2018
   Pin Change Interrupts - Single Rx Channel i.e Single PWM Wave
  PCMSKx - Pin Change Mask Register
  PCMSK0 - portB (D8-D13) (PCINT0  - PCINT6)
  PCMSK1 - portC (A0-A5)  (PCINT8  - PCINT14)
  PCMSK2 - portD (D0-D7)  (PCINT16 - PCINT23)
  PCICR -  Pin Change Interrupt Control Register
    PCIE0 - Pin Change Interrupt Enable 0 - Port B (D8-D13)
    PCIE1 - Pin Change Interrupt Enable 1 - Port C (A0-A5)
    PCIE2 - Pin Change Interrupt Enable 2 - Port D (D0-D7)
*/

/*#############################
  Rx Connections
  Ch1 = D8 PCINT0
  Ch2 = D9 PCINT1
  Ch3 = D10 PCINT2
  Ch4 = D11 PCINT3
*/
#define DEBUG // comment it out to disable the debug mode

volatile boolean recvPCInt = false; //to know interrupt status
volatile int portValue;

//used to store the pwm duration
volatile unsigned long pwmDuration[4];
volatile unsigned long pwmStart[4];
unsigned long pwmEnd[4];

//pinDeclaration for Rx
const byte rxCh[] = {8, 9, 10, 11};
const byte noOfChannels = sizeof(rxCh);

//portStatus
volatile int prevPortState[] = {0, 0, 0, 0};
volatile int presentPortState[4];

//Interrupt Service Routine will fire when for PinChange in PortB
ISR(PCINT0_vect) {
  recvPCInt = true;
  for (int ch = 0; ch < noOfChannels ; ch++) {
    presentPortState[ch] = digitalRead(rxCh[ch]);
  }//end of for loop

  for (int c = 0; c < noOfChannels ; c++) {
    if (prevPortState[c] == 0 & presentPortState[c] == 1) {
      //if previous state is 0 and present state is 1 (Raising Edge) then take the time stamp
      pwmStart[c] = micros();
      prevPortState[c] = 1; //update the prevPort State
    } else if (prevPortState[c] == 1 & presentPortState[c] == 0) {
      //if previous state is 1 and present state is 0 (Falling Edge) then calculate the width based on the change
      pwmDuration[c] = micros() - pwmStart[c];
      prevPortState[c] = 0; //update Present PortState
    }
  }//end of for loop
  //portValue = PINB & 0x0f;//we are only intrested in first four bits
}

void setup() {
  cli(); //Clear all interrupts
  PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13
  PCMSK0 |= (1 << PCINT0) | (1 << PCINT1) | (1 << PCINT2) | (1 << PCINT3);// Pin8,9,10,11
  sei(); //enable all interrupts

#ifdef DEBUG
  Serial.begin(9600);
  Serial.println("DEBUG mode Enabled");
#endif

  for (int ch = 0 ; ch < noOfChannels; ch++) {
    pinMode(rxCh[ch], INPUT_PULLUP); //make pin input with pullup enabled
  }//end of for loop

}//end of setup

void checkForRxPulseWidths() {
  for (int i = 0; i < noOfChannels ; i++ ) {
#ifdef DEBUG
    Serial.print("ch => ");
    Serial.print(i);
    Serial.print(" pulseWidth ");
    Serial.println(pwmDuration[i]);
#endif
  }
}

void loop() {
  if (recvPCInt == true) {
    checkForRxPulseWidths();
    recvPCInt = false;
  }//end of if
}//end of loop

Reading PWM Signals using Pin Change Interrupts

To Measure a PWM wave using Arduino we can follow any one of following ways

1.  built-in pulseIn function
2. Timer and External Interrupts
3. Pin Change Interrupts

Below Code is used to measure PWM width when connected to pin 9

//   Pin Change Interrupts
//PCMSKx - Pin Change Mask Register
//PCMSK0 - portB (D8-D13) (PCINT0  - PCINT6)
//PCMSK1 - portC (A0-A5)  (PCINT8  - PCINT14)
//PCMSK2 - portD (D0-D7)  (PCINT16 - PCINT23)
//PCICR -  Pin Change Interrupt Control Register
//  PCIE0 - Pin Change Interrupt Enable 0 - Port B (D8-D13)
//  PCIE1 - Pin Change Interrupt Enable 1 - Port C (A0-A5)
//  PCIE2 - Pin Change Interrupt Enable 2 - Port D (D0-D7)

volatile boolean recvPCInt = false;
volatile byte portValue;
unsigned long pwmDuration;
unsigned long pwmStart;
unsigned long pwmEnd;


//Interrupt Service Routine will fire when for PinChange in PortB
ISR(PCINT0_vect){
  recvPCInt = true;
  portValue = PINB; //PINB is used to read all port input values
}

void setup() {
cli(); //Clear all interrupts
PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13
PCMSK0 |= 1 << PCINT1;// Pin9
sei(); //enable all interrupts
pinMode(9,INPUT);
digitalWrite(9,HIGH);//enable pull up in pin 9
Serial.begin(9600);
}

void loop() {
  if(recvPCInt == true){
     boolean event = digitalRead(9);
     if(event == HIGH)
        {
          pwmStart = micros();
        }
       else if(event == LOW){
          pwmEnd = micros();
        Serial.println(pwmEnd-pwmStart);
       }
     recvPCInt = false;
  }
}