Project finished and updates

Well it’s been a while since I’ve posted here; as the deadline for finishing this project for the Arkwright Scholarship approached, I stopped logging my progress because it was taking too long and I had to prioritise finishing the project.

I finished both the drone itself and the code, and though I never actually got it to fly well (before dismantling it) I could see it was just a matter of PID tuning to get it to work well. Another thing I would definitely add in the future is GPS tracking; I got the feeling that a lot of the drift I was experiencing was just the wind and not the angle of the quad itself.

So here’s a quick retrospective summary of what I did after that last post.

After printing the arms and assembling the main frame, I changed my plan for a battery undercarriage because it became clear that having the arms so high above the bottom of the unit would make it far too unstable. Instead, at the cost of a higher centre of gravity, I designed a lid with a built-in battery holder, which ended up working very nicely. This can be seen on the renders and images of the whole drone I’ll include below.

After complete assembly – which proved very tricky indeed, due to the rather inconvenient positioning of screw fittings (something I’ll remember in the future) the next thing to do was finalise the code.

I regret that I didn’t document this fully at the time, but I’m not going to get my head stuck all the way back into it just for the sake of writing it up now. So, sufficient to say, I spent a while finishing all the sections of the code and fitting them all together. In the end, it all came to 840 lines including comments, and that’s after adding extensive feedback and safety systems (for failsafe altitude hold, prevention of motors spinning when user isn’t ready (for which I built an arm/disarm system using a button combination), serial debug output, etc). The full code is inserted here and attached below.

/*
Arduino quadrotor flight controller. Written for application to the 2015/2016 Arkwright Engineering Scholarship.
More information at http://dfalck.xyz/arduino-quadcopter

Copyright (C) 2016  D. Falck

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

The Full GNU General Public License, under which this program is licensed, is available at http://www.gnu.org/licenses/gpl.html.
*/

/*  Correct usage: the program is loaded to an Arduino (tested on Uno R3 SMD) and the following peripherals are attached:
 *    Four brushless outrunner motors (~1000RPM/V recommended) with electronic speed controllers
 *    Slow-fly propellers (10-inch diameter with a 4.5-inch pitch recommended)
 *    A four to six channel 2.4GHz RC receiver (paired with a corresponding transmitter)
 *    A Polulu AltIMU-v10.4 inertial measurement unit connected to SDA and SCL pins and positioned with the silk screen on top and the text facing to the right of the forwards flight direction
 *    (Other sensors can be used but the ReadSensors page and related items will have to be edited accordingly)
 *    A buzzer or LED on pin 13 (NB it is recommended that a solderable breadboard or custom PCB be used for mounting of the inertial measurement unit and buzzer.)
 *    A power source: 3-cell lithium polymer battery recommended with low voltage alarm. To be connected in parallel with speed controllers.
 *  The quadrotor should be approximately 550mm in diameter with the motors arranged in an X formation (two front, two back).
 *  The front-right and back-left propellers should spin anticlockwise, and the front-left and back-right propellers should spin clockwise. To switch motor direction, swap any two wires between motor and speed controller.
 *  Connection pins can be seen in the program definitions section below.
 *  The centre of gravity of the quadcopter should be as near the centrepoint as possible.
 *  Note: The PID constants for the Pitch, Roll and Yaw rate controllers may have to be adjusted. The stabilise controllers should not be altered.
 *  For more information visit http://dfalck.xyz/arduino-quadcopter
 */

/* Program behaviour:
 * Upon startup the buzzer emits a long tone to signify that the flight controller is ready.
 * Individual motors can be tested at 10% power using serial commands 'mfl', 'mfr', 'mbl' and 'mbr' respectively, followed by 's' to stop the motor.
 * The serial command 'k' initiates an emergency stop, following which 'b' will resume the main program.
 * Other commands and serial feedback can be created and altered in the SerialInterface section.
 * The quadrotor should be placed on a flat, stable surface for take-off.
 * The motors are armed by moving both joysticks on the transmitter to the bottom-left position for 2 seconds. A long tone followed by two short tones indicates that the motors are armed.
 * Upon arming, all four motors will spin to a base rate of 10% power.
 * The same process can be repeated to disarm the motors upon landing.
 * On a standard mode-2 RC transmitter, the left joystick controls throttle (vertical) and yaw/rudder (horizontal) and the right joystick controls pitch/elevator (vertical) and roll/aileron (horizontal).
 * The quadrotor is limited to 150deg/s maximum rotation rate in yaw and 45deg maximum tilt angle in pitch and roll.
 * The throttle directly controls the average speed of the motors, no altitude hold is implemented. The user should thus take care when adjusting the throttle value.
 * In the event of receiver disconnection during flight the quadrotor will hold position at the throttle value defined by 'FAILSAFE_THROTTLE' and emit a continuous tone until signal is reacquired.
 */
 
//INCLUSIONS:

  //ReadSensors inclusions:

    #include <Wire.h> //12C sensor interface
    #include <L3G.h> //Gyro library
    #include <LSM303.h> //Accel library

  //ReadRC inclusions:

    #include <EnableInterrupt.h> //Library for seamless pin change interrupts

  //PIDLoops inclusions:

    #include <PID_v1.h> //For PID controllers

  //MotorOut inclusions:

    #include <Servo.h> //To control motors

//DEFINITIONS:

  //SensorFusion definitions:

    #define FILTER_FREQ 10000 //Frequency to update filter at, in microseconds

  //ReadRC definitions:

    #define RCTHR 4 //RC input pins
    #define RCYAW 12
    #define RCPIT 7
    #define RCROL 8
    #define RCAUX1 5
    #define RCAUX2 6

  //MotorOut definitions:
  
    #define MOTOR_FL 3 //Motor output pins
    #define MOTOR_FR 9
    #define MOTOR_BL 10
    #define MOTOR_BR 11

  //PIDLoops definitions:

    #define PITCH_RATE_KP 1 //PID controller constants
    #define PITCH_RATE_KI 0
    #define PITCH_RATE_KD 0
    #define ROLL_RATE_KP 1
    #define ROLL_RATE_KI 0
    #define ROLL_RATE_KD 0
    #define YAW_RATE_KP 2.5
    #define YAW_RATE_KI 0
    #define YAW_RATE_KD 0
    #define PITCH_STAB_KP 4.5
    #define PITCH_STAB_KI 0
    #define PITCH_STAB_KD 0
    #define ROLL_STAB_KP 4.5
    #define ROLL_STAB_KI 0
    #define ROLL_STAB_KD 0

    #define PITCH_RATE_MIN -500 //Min and max PID output values to cap the controllers' outputs
    #define PITCH_RATE_MAX 500
    #define ROLL_RATE_MIN -500
    #define ROLL_RATE_MAX 500
    #define YAW_RATE_MIN -500
    #define YAW_RATE_MAX 500
    #define PITCH_STAB_MIN -250
    #define PITCH_STAB_MAX 250
    #define ROLL_STAB_MIN -250
    #define ROLL_STAB_MAX 250

    #define PID_SAMPLETIME 10 //How frequently the PID loops evaluate, in milliseconds

  //ReadRC definitions:

    #define THROTTLE_RAW_MIN 1168 //Min and max raw RC values (in us of signal width) aka joystick endpoints. Can be calibrated using serial print of globals throttleRaw, yawRaw etc. and moving the joysticks to their end positions
    #define THROTTLE_RAW_MAX 1832
    #define YAW_RAW_MIN 1136
    #define YAW_RAW_MAX 1912
    #define PITCH_RAW_MIN 1156
    #define PITCH_RAW_MAX 1808
    #define ROLL_RAW_MIN 1140
    #define ROLL_RAW_MAX 1896
    #define AUX1_RAW_MIN 1000
    #define AUX1_RAW_MAX 2024
    #define AUX2_RAW_MIN 1000
    #define AUX2_RAW_MAX 2032

    #define FAILSAFE_THROTTLE 1000 //Throttle value to implement upon failsafe

  //SerialInterface definitions:

    #define BAUDRATE 115200 //Serial baud rate
    #define SERIAL_FREQ 500 //Frequency of serial output, in milliseconds

  //BuzzerControl definitions:

    #define BUZZER_PIN 13 //Pin to which the buzzer is connected

//CUSTOM VARIABLE TYPES:
  
  typedef struct { //Creating variable type to hold all mapped RC values
    double throttle;
    double yaw;
    double pitch;
    double roll;
    double aux1;
    double aux2;
  }RCData;
  
  typedef struct { //Creating variable type to hold all position data (Euler angles)
    double x;
    double y;
    double z;
  }XYZData;

//VARIABLES:

  //ReadSensors variables:

    L3G gyro; //create instance of gyro library
    LSM303 accel; //create instance of accel library

  //SensorFusion variables:

    unsigned long lastFilter; //for timekeeping, in microseconds
    unsigned long lastProduce; //Ditto
    
    double filteredAngleX; //Global variables, altered by complementary filter, give current X and Y angle, in deg
    double filteredAngleY; //Ditto
    double gyroRateX; //Current rotation rate given by gyro, in deg/s
    double gyroRateY; //Ditto
    double gyroRateZ; //Ditto
    //Orientation info: Y tilt up positive, X tilt right positive, Z turn right positive

  //ReadRC variables:
  
    volatile unsigned long throttleStart, yawStart, pitchStart, rollStart, aux1Start, aux2Start; //variables for the timestamp of the start of a rising signal
    volatile unsigned long throttleRaw, yawRaw, pitchRaw, rollRaw, aux1Raw, aux2Raw; //variables for the length of high signal

    boolean failsafe = false; //Failsafe mode on?
    boolean rcOff = false; //RC dead?

    bool armed = false; //MUST START FALSE!!!!!!!!!!
    
    bool arming = false;
    bool stickcombo = false;

    unsigned long armStart;
    
    RCData currentRC; //Global RC data holder

  //PIDLoop variables:
  
    RCData RPIDOut; //Rate PIDs
    RCData SPIDOut; //Stabilise PIDs

    //Construct rate PIDs (control motors given desired rate (stab controllers) and actual rate (gyros))
    PID pitchRPID(&gyroRateY, &RPIDOut.pitch, &SPIDOut.pitch, PITCH_RATE_KP, PITCH_RATE_KI, PITCH_RATE_KD, DIRECT); //PID constructors have format myPID(input, output, setpoint, kp, ki, kd, direction)
    PID rollRPID(&gyroRateX, &RPIDOut.roll, &SPIDOut.roll, ROLL_RATE_KP, ROLL_RATE_KI, ROLL_RATE_KD, DIRECT);

    //Construct Yaw rate PID (control motors given desired rate (RC) and actual rate (gyros))
    PID yawRPID(&gyroRateZ, &RPIDOut.yaw, &currentRC.yaw, YAW_RATE_KP, YAW_RATE_KI, YAW_RATE_KD, DIRECT);
    
    //Construct stabilise PIDs (control desired rate given desired angle (RC) and actual angle (sensors))
    PID pitchSPID(&filteredAngleY, &SPIDOut.pitch, &currentRC.pitch, PITCH_STAB_KP, PITCH_STAB_KI, PITCH_STAB_KD, DIRECT);
    PID rollSPID(&filteredAngleX, &SPIDOut.roll, &currentRC.roll, ROLL_STAB_KP, ROLL_STAB_KI, ROLL_STAB_KD, DIRECT);

  //MotorOut variables:

    Servo motorFrontLeft;
    Servo motorFrontRight;
    Servo motorBackLeft;
    Servo motorBackRight;

    double motorFLOut; //Motor output value holders
    double motorFROut;
    double motorBLOut;
    double motorBROut;

    boolean flying = false; //Are the motors spinning?

  //SerialInterface variables:

    unsigned long lastPrint;
    
//MAIN SETUP:

  void setup() {
    setupBuzzer();
    setupSensors();
    setupMotors();
    setupRC();
    setupFusion();
    setupPID();
    setupSerial();
    buzzerOn();
    delay(1000);
    buzzerOff();
  }

//MAIN LOOP:

  void loop() {
    armDisarm();
    produceFilteredAngles(); //Every FILTER_FREQ, reads sensors, carries out fusion and stores current X and Y filtered angles in globals filteredAngleX and filteredAngleY, and current gyro Z rate in global gyroRateZ
    rcMap(); //Maps the last values for all RC channels (generated by interrupts) correctly and stores them in global currentRC.throttle, .pitch, .roll, .yaw, .aux1 and .aux2, also carries out failsafe routines
    runPID(); //Runs all PID controllers
    motorOutput(); //Outputs determined values to motors, decides whether to power motors
    runSerialCommands(); //Receive commands over serial
    runSerialOutput(); //Print useful values to serial
    buzzerOutput();
  }

//ALL AUXILIARY FUNCTIONS

/* BuzzerControl functions
 * Sets up the buzzer and runs it when indicated by certain program events
 * 'setupBuzzer()' sets up the buzzer and should be run once in the setup function
 * 'buzzerOn()' turns the buzzer on
 * 'buzzerOff()' turns the buzzer off
 * 'buzzerControl()' controls the buzzer based on certain events referenced elsewhere.
 * N.B. The buzzer can also be accessed and controlled in other sections of code
 */

void setupBuzzer() {
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);
}

void buzzerOn() {
  digitalWrite(BUZZER_PIN, HIGH);
}

void buzzerOff() {
  digitalWrite(BUZZER_PIN, LOW);
}

void buzzerOutput() {
  if(failsafe) {
    buzzerOn();
  } else buzzerOff();
}

/* MotorOut functions
 * Sets up motors and outputs the values to the motors determined from the PID loops. Also contains logic for when to turn motors on/off etc.
 * 'setupMotors()' initialises the motors, assigns them to a servo controller and turns them on at 1000us (0% output), and should be run once in the setup function.
 * 'motorOutput()' runs the motors at the values determined from the PID loops, as well as deciding when to and when not to run them, and should be run every loop.
 */

void setupMotors() {
  pinMode(MOTOR_FR, OUTPUT); //Set the motor pins as outputs
  pinMode(MOTOR_FL, OUTPUT);
  pinMode(MOTOR_BL, OUTPUT);
  pinMode(MOTOR_BR, OUTPUT);
  
  motorFrontLeft.attach(MOTOR_FL, 1000, 2000); //Attach servo controller to defined ports, with min pulse width 1000us and max 2000us (standard RC pulse widths for reading by ESC).
  motorFrontRight.attach(MOTOR_FR, 1000, 2000);
  motorBackLeft.attach(MOTOR_BL, 1000, 2000);
  motorBackRight.attach(MOTOR_BR, 1000, 2000);
  
  motorFrontLeft.writeMicroseconds(1000); //Turns all motors on at 1000us (0% output)
  motorFrontRight.writeMicroseconds(1000);
  motorBackLeft.writeMicroseconds(1000);
  motorBackRight.writeMicroseconds(1000);
  
  motorFLOut = 1000; //Sets all motor output variables to 1000us (0% output)
  motorFROut = 1000;
  motorBLOut = 1000;
  motorBROut = 1000;
}

void motorOutput() {
  if (armed) { //Only power motors if armed
    if (currentRC.throttle >= 1100) { //If throttle not at bottom, set the motor variables to the determined values
      motorFLOut = currentRC.throttle + RPIDOut.pitch + RPIDOut.roll - RPIDOut.yaw;
      motorFROut = currentRC.throttle + RPIDOut.pitch - RPIDOut.roll + RPIDOut.yaw;
      motorBLOut = currentRC.throttle - RPIDOut.pitch + RPIDOut.roll + RPIDOut.yaw;
      motorBROut = currentRC.throttle - RPIDOut.pitch - RPIDOut.roll - RPIDOut.yaw;
    } else { //If throttle at bottom (but still armed), set motor variables to 5%
      motorFLOut = 1100;
      motorFROut = 1100;
      motorBLOut = 1100;
      motorBROut = 1100;
    }
  } else { //If disarmed, set motor variables to 0%
    motorFLOut = 1000;
    motorFROut = 1000;
    motorBLOut = 1000;
    motorBROut = 1000;
  }
  
  if (motorFLOut > 1100 && motorFROut > 1100 && motorBLOut > 1100 && motorBROut > 1100) { //Set global bool 'flying' to true if all motors powered and above armed base rate (1100), otherwise set to false. For use in failsafe
    flying = true;
  } else flying = false;

  motorFrontLeft.writeMicroseconds(motorFLOut); //Run motors at values set to motor variables
  motorFrontRight.writeMicroseconds(motorFROut);
  motorBackLeft.writeMicroseconds(motorBLOut);
  motorBackRight.writeMicroseconds(motorBROut);
  
}

/* PIDLoops functions
 * Sets up and runs the main PID loops for control of the quadcopter.
 * 'setupPID()' sets the output limits and sample times of the controllers and initialises them, and should be run once in the setup function.
 * 'runPID()' computes the PID controllers at the set sample frequency, and should be run every loop.
 */

void setupPID() {
  //Output limits for rate controllers
  pitchRPID.SetOutputLimits(PITCH_RATE_MIN, PITCH_RATE_MAX);
  rollRPID.SetOutputLimits(ROLL_RATE_MIN, ROLL_RATE_MAX);
  yawRPID.SetOutputLimits(YAW_RATE_MIN, YAW_RATE_MAX);
  
  //Output limits for stabilise controllers
  pitchSPID.SetOutputLimits(PITCH_STAB_MIN, PITCH_STAB_MAX);
  rollSPID.SetOutputLimits(ROLL_STAB_MIN, ROLL_STAB_MAX);

  //Sample times for rate controllers
  pitchRPID.SetSampleTime(PID_SAMPLETIME);
  rollRPID.SetSampleTime(PID_SAMPLETIME);
  yawRPID.SetSampleTime(PID_SAMPLETIME);

  //Sample times for stabilise controllers
  pitchSPID.SetSampleTime(PID_SAMPLETIME);
  rollSPID.SetSampleTime(PID_SAMPLETIME);

  //Begin running rate controllers
  pitchRPID.SetMode(AUTOMATIC);
  rollRPID.SetMode(AUTOMATIC);
  yawRPID.SetMode(AUTOMATIC);

  //Begin running stabilise controllers
  pitchSPID.SetMode(AUTOMATIC);
  rollSPID.SetMode(AUTOMATIC);
}

void runPID() {
  //Compute rate controllers (every sample time specified)
  pitchRPID.Compute();
  rollRPID.Compute();
  yawRPID.Compute();

  //Compute stabilise controllers
  pitchSPID.Compute();
  rollSPID.Compute();
}

/* ReadRC functions
 * Reads values of 6 RC input channels using software interrupts.
 * The current raw PWM value of all six channels is stored in globals throttleRaw, yawRaw, pitchRaw, rollRaw, aux1Raw, aux2Raw.
 * The mapped values (see function rcMap()) are stored in global variable currentRC.
 * 'setupRC()' function initialises the RC pins and should be called once in the setup function.
 * 'rcMap()' maps the latest RC values (from interrupts) to global currentRC, and should be run once a cycle to get the new current mapped values.
 * 'failGuard()' contains failsafe guarding and is called by rcMap(). If no data is received from the TX for a second, and the motors are on, all channels are set to centre.
 * 'armDisarm()' controls boolean 'armed' based on stick combo from transmitter, and should be run every loop.
 * All other functions are used by interrupts and should not be touched.
*/

void setupRC() {
  pinMode(RCTHR, INPUT); //Set RC input pins as INPUT
  pinMode(RCYAW, INPUT);
  pinMode(RCPIT, INPUT);
  pinMode(RCROL, INPUT);
  pinMode(RCAUX1, INPUT);
  pinMode(RCAUX2, INPUT);
  
  enableInterrupt(RCTHR, interruptThrottleRise, RISING); //Attach interrupt function for a rising signal
  enableInterrupt(RCYAW, interruptYawRise, RISING);
  enableInterrupt(RCPIT, interruptPitchRise, RISING);
  enableInterrupt(RCROL, interruptRollRise, RISING);
  enableInterrupt(RCAUX1, interruptAux1Rise, RISING);
  enableInterrupt(RCAUX2, interruptAux2Rise, RISING);
}

void rcMap() { //Map latest RC raw values to useable ranges
  //Throttle
  if (throttleRaw == 0) currentRC.throttle = 0;
  else currentRC.throttle = constrain(map(throttleRaw, THROTTLE_RAW_MIN, THROTTLE_RAW_MAX, 1000, 2000), 1000, 2000); //Throttle 1000-2000 for standard RC limits

  //Yaw
  if (yawRaw == 0) currentRC.yaw = 0;
  else {
    currentRC.yaw = constrain(map(yawRaw, YAW_RAW_MIN, YAW_RAW_MAX, -150, 150), -150, 150); //Yaw -150-150 for angle rate control
    if (abs(currentRC.yaw) <= 10) currentRC.yaw = 0; //If untouched, keep in centre
  }

  //Pitch
  if (pitchRaw == 0) currentRC.pitch = 0;
  else {
    currentRC.pitch = -constrain(map(pitchRaw, PITCH_RAW_MIN, PITCH_RAW_MAX, -45, 45), -45, 45); //Pitch -45-45 for angle control
    if (abs(currentRC.pitch) <= 5) currentRC.pitch = 0;
  }

  //Roll
  if (rollRaw == 0) currentRC.roll = 0;
  else {
    currentRC.roll = constrain(map(rollRaw, ROLL_RAW_MIN, ROLL_RAW_MAX, -45, 45), -45, 45); //Roll -45-45 for angle control
    if (abs(currentRC.roll) <= 5) currentRC.roll = 0;
  }

  //Aux 1 (left knob)
  if (aux1Raw == 0) currentRC.aux1 = 0;
  else currentRC.aux1 = constrain(map(aux1Raw, AUX1_RAW_MIN, AUX1_RAW_MAX, 0, 100), 0, 100); //Aux1 knob 0-100 for easy use

  //Aux 2 (right knob)
  if (aux2Raw == 0) currentRC.aux2 = 0;
  else currentRC.aux2 = constrain(map(aux2Raw, AUX2_RAW_MIN, AUX2_RAW_MAX, 0, 100), 0, 100); //Aux2 knob 0-100 for easy use

  failGuard();
}

void failGuard() { //Failsafe procedures
  unsigned long now = micros();
  if ((now - throttleStart > 500000 && now - yawStart > 500000 && now - pitchStart > 500000 && now - rollStart > 500000 && now - aux1Start > 500000 && now - aux2Start > 500000) || (throttleRaw == 0 && pitchRaw == 0 && rollRaw == 0 && yawRaw == 0 && aux1Raw == 0 && aux2Raw == 0)) { //No RC signal for half a second (turned off) OR all RC channels 0 (initial off)
    rcOff = true;
  } else rcOff = false;

  if (rcOff && flying) { //Activate failsafe
    failsafe = true;
  } else failsafe = false;

  if (failsafe) { //If failsafe active
    currentRC.throttle = FAILSAFE_THROTTLE; //Set all flight RC to centre
    currentRC.pitch = 0;
    currentRC.roll = 0;
    currentRC.yaw = 0;
  }
}

void armDisarm() { //arm is synonymous with disarm here except for the boolean 'armed'
  if (!rcOff && !flying && throttleRaw < THROTTLE_RAW_MIN + 100 && yawRaw < YAW_RAW_MIN + 100 && pitchRaw < PITCH_RAW_MIN + 100 && rollRaw < ROLL_RAW_MIN + 100) {//Sticks in right combo position to arm/disarm
    stickcombo = true;
  } else {
    stickcombo = false;
    arming = false; //Cancel any arming/disarming if !stickcombo
  }
  
  if (!arming && stickcombo) { //Trying to arm
    arming = true;
    armStart = millis();
  }
  
  if (arming && stickcombo && millis() - armStart > 1000) { //Arming for 1-1.5sec
    armed = !armed; //Toggle armed or disarmed
    arming = false;
    buzzerOn(); //Indicator beep: this stops the program temporarily but shouldn't matter as a condition is !flying
    delay(1000);
    buzzerOff();
    delay(250);
    buzzerOn();
    delay(250);
    buzzerOff();
  }
}

/////////////////////////////////
//INTERRUPT FUNCTIONS:
/////////////////////////////////

void interruptThrottleRise() {
  enableInterrupt(RCTHR, interruptThrottleFall, FALLING); //Attach the other function for when it next falls again
  throttleStart = micros();
}

void interruptYawRise() {
  enableInterrupt(RCYAW, interruptYawFall, FALLING);
  yawStart = micros();
}

void interruptPitchRise() {
  enableInterrupt(RCPIT, interruptPitchFall, FALLING);
  pitchStart = micros();
}

void interruptRollRise() {
  enableInterrupt(RCROL, interruptRollFall, FALLING);
  rollStart = micros();
}

void interruptAux1Rise() {
  enableInterrupt(RCAUX1, interruptAux1Fall, FALLING);
  aux1Start = micros();
}

void interruptAux2Rise() {
  enableInterrupt(RCAUX2, interruptAux2Fall, FALLING);
  aux2Start = micros();
}

void interruptThrottleFall() {
  enableInterrupt(RCTHR, interruptThrottleRise, RISING); //Attach the other function for when it next rises again
  throttleRaw = micros() - throttleStart;
}

void interruptYawFall() {
  enableInterrupt(RCYAW, interruptYawRise, RISING);
  yawRaw = micros() - yawStart;
}

void interruptPitchFall() {
  enableInterrupt(RCPIT, interruptPitchRise, RISING);
  pitchRaw = micros() - pitchStart;
}

void interruptRollFall() {
  enableInterrupt(RCROL, interruptRollRise, RISING);
 rollRaw = micros() - rollStart;
}

void interruptAux1Fall() {
  enableInterrupt(RCAUX1, interruptAux1Rise, RISING);
 aux1Raw = micros() - aux1Start;
}

void interruptAux2Fall() {
  enableInterrupt(RCAUX2, interruptAux2Rise, RISING);
 aux2Raw = micros() - aux2Start;
}

/* ReadSensors functions
 * Sets up gyro and accel, reads raw values and converts them to dps and g
 * These are then used in the SensorFusion functions to produce useable values
 * 'setupSensors()' sets up sensor board
 * 'gyroReadDPS()' returns current gyro values in dps, type XYZData
 * 'accelReadG()' returns current accel values in g, type XYZData
 */

void setupSensors() {
  Wire.begin(); //Start receiving 12C data

  if (!gyro.init()) //Initialise gyro, exit if fail
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }

  gyro.enableDefault(); //Setup gyro

  accel.init(); //Initialise accel
  accel.enableDefault(); //Setup accel
}

XYZData gyroReadDPS() {
  XYZData rtval; //Variable to store the returnable values
  
  gyro.read();

  //Account for gyro offset/bias (using average measured from average finding sketch),
  //and conversion from raw values to dps (degrees per second)
  rtval.x = (gyro.g.x + 95.09) * 8.75 / 1000.0;
  rtval.y = (gyro.g.y + 548.14) * 8.75 / 1000.0;
  rtval.z = (gyro.g.z - 5.29) * 8.75 / 1000.0;

  return rtval;
}

XYZData accelReadG() {
  XYZData rtval; //Variable to store the returnable values
  
  accel.read();

  //Conversion from raw values to g
  rtval.y = accel.a.x * 0.061 / 1000.0; //X and Y values swapped for portrait orientation of sensor, new X inverted
  rtval.x = map(accel.a.y, -1, 1, 1, -1) * 0.061 / 1000.0;
  rtval.z = accel.a.z * 0.061 / 1000.0;

  return rtval;
}

/* SensorFusion functions
 * Converts accel g values to Euler angles, and filters accel and gyro values using complementary filter to produce estimated Euler angles, as well as saving the raw gyro rates.
 * 'setupFusion()' sets up the timekeeping variable for the filter and should be run once in a setup function
 * 'accelToDeg()' takes a double, converts the acceleration due to gravity to an angle and returns it in degrees as a double
 * 'complementaryFilter()' takes X and Y accel angles and gyro rates, and outputs a filtered angle to globals filteredAngleX and filteredAngleY
 * 'produceFilteredAngles' is the main loopable function. Every FILTER_FREQ it reads the sensors, calls the filter and outputs globals filteredAngleX, filteredAngleY, gyroRateX, gyroRateY and gyroRateZ.
 */

void setupFusion() {
  lastFilter = micros();
  lastProduce = micros();
}

double accelToDeg(double acceleration) {
  acceleration = constrain(acceleration, -1, 1);
  return asin(acceleration)*180.0/PI;
}

void complementaryFilter(double accelAngleX, double gyroRateX, double accelAngleY, double gyroRateY) {
  unsigned long now = micros();
  
  filteredAngleX = 0.98 * (filteredAngleX + gyroRateX * ((now - lastFilter) / 1000000.0)) + 0.02 * accelAngleX; //'Complementary' or 'balance' sensor filter: high-passes integrated gyro data, low-passes accelerometer data
  filteredAngleY = 0.98 * (filteredAngleY + gyroRateY * ((now - lastFilter) / 1000000.0)) + 0.02 * accelAngleY;
  
  lastFilter = micros();
}

void produceFilteredAngles() {
  unsigned long now = micros();
  
  if(now - lastFilter >= FILTER_FREQ) {
    XYZData gyroval = gyroReadDPS(); //Create holder for read values
    XYZData accelval = accelReadG();
    
    complementaryFilter(accelToDeg(accelval.x), gyroval.x, accelToDeg(accelval.y), gyroval.y); //Filter the values to globals filteredAngleX and filteredAngleY

    gyroRateX = gyroval.x; //Parse gyro values to globals
    gyroRateY = gyroval.y;
    gyroRateZ = gyroval.z;
  }
}

/* SerialInterface functions
 * Allows control of certain items or changing of tuning constants over serial (baud rate 115200, or other if defined differently), and continuous printing of useful values to serial
 * 'runSerialCommands()' evaluates incoming serial data and should be run once every loop
 * 'runSerialOutput()' is used to continuously output key values needed on serial, should be run every loop, and should be configured as necessary. Several possible configurations are commented for future use.
 * To test motors, type 'm' followed immediately by 'fl, fr, bl or br', corresponding to the motor desired. The motor will run at 1100us (10%). To stop the motor type 's'.
 * To change tuning parameters, first EDIT THE CODE to reflect the parameters wanted to be changed, then type 't' followed immediately by the
 * desired letters to indicate which parameter to change, followed after a space by the new value.
 * N.B. new parameters will NOT save. They should be noted from the serial feedback and changed manually in the main definitions.
  */

void setupSerial() {
  Serial.begin(BAUDRATE);
  lastPrint = millis();
  Serial.println(F("Starting"));
}

void runSerialOutput() { //Uncomment required print statements, or create new ones.
  unsigned long now = millis();
  
  if(now - lastPrint >= SERIAL_FREQ) { //Print everything every SERIAL_FREQ milliseconds    
    if (failsafe == true) { //Notify if failsafe is active: DO NOT REMOVE
      Serial.println("FAILSAFE ACTIVE");
    }

    if (armed) Serial.print(F("Armed. "));
    Serial.print(F("Sensors: Filtered X Angle: "));
    Serial.print(filteredAngleX);
    Serial.print(F(" Filtered Y Angle: "));
    Serial.print(filteredAngleY);
    Serial.print(F(" Gyro Z Rate: "));
    Serial.print(gyroRateZ);
    Serial.print(F("             RC Channels: Pitch: "));
    Serial.print(currentRC.pitch);
    Serial.print(F(" Roll: "));
    Serial.print(currentRC.roll);
    Serial.print(F(" Yaw: "));
    Serial.print(currentRC.yaw);
    Serial.print(F(" Throttle: "));
    Serial.print(currentRC.throttle);
    Serial.print(F(" Aux1: "));
    Serial.print(currentRC.aux1);
    Serial.print(F(" Aux2: "));
    Serial.print(currentRC.aux2);
    /*Serial.print(F("Pitch Rotation "));
    Serial.print(filteredAngleY);
    Serial.print(F(" Desired: "));
    Serial.print(currentRC.pitch);
    Serial.print(F(" Output: "));
    Serial.println(RPIDOut.pitch);*/
    Serial.print(F("               Motors: FL: "));
    Serial.print(motorFLOut);
    Serial.print(F(" FR: "));
    Serial.print(motorFROut);
    Serial.print(F(" BL: "));
    Serial.print(motorBLOut);
    Serial.print(F(" BR: "));
    Serial.println(motorBROut);
    Serial.print(F("              Pitch Rate KP: "));
    Serial.print(pitchRPID.GetKp());
    Serial.print(F(" Roll Rate KP: "));
    Serial.print(rollRPID.GetKp());
    Serial.print(F(" Yaw Rate KP: "));
    Serial.println(yawRPID.GetKp());
    lastPrint = now;
  }
}

void runSerialCommands() {
  if (Serial.available() > 0) { //Closed if statement for emergency stop
    if (Serial.peek() == 'k') { //Emergency stop
      Serial.read();
      motorFrontLeft.writeMicroseconds(1000);
      motorFrontRight.writeMicroseconds(1000);
      motorBackLeft.writeMicroseconds(1000);
      motorBackRight.writeMicroseconds(1000);
      Serial.println(F("EMERGENCY STOP"));
      while(Serial.read() != 'b'); //Start again after emergency stop
      Serial.println(F("RESTART"));
    }
  }

  if (Serial.available() > 0) { //If statement for all other commands (except emergency stop).
    String command = Serial.readString();
    
    switch (command.charAt(0)) {
      case 't': //Tune PID constants
        switch (command.charAt(1)) {
          case 'p': { //Pitch PID rate P constant
            String newval = command.substring(3);
            float newfloat = newval.toFloat();
            pitchRPID.SetTunings(newfloat, pitchRPID.GetKi(), pitchRPID.GetKd());
            Serial.print(F("New Pitch Rate P constant is "));
            Serial.println(pitchRPID.GetKp());
            break;
          }
          case 'r': {//Roll PID rate P constant
            String newval = command.substring(3);
            float newfloat = newval.toFloat();
            rollRPID.SetTunings(newfloat, rollRPID.GetKi(), rollRPID.GetKd());
            Serial.print(F("New Roll Rate P constant is "));
            Serial.println(rollRPID.GetKp());
            break;
          }
          case 'y': { //Yaw PID rate P constant
            String newval = command.substring(3);
            float newfloat = newval.toFloat();
            yawRPID.SetTunings(newfloat, yawRPID.GetKi(), yawRPID.GetKd());
            Serial.print(F("New Yaw Rate P constant is "));
            Serial.println(yawRPID.GetKp());
            break;
          }
          default:
            Serial.println(F("Tuning loop name not recognised"));
            break;
        }
        break;
        
      case 'm': //Motor testing
        switch (command.charAt(1)) {
          case 'f': //Front motor
            switch (command.charAt(2)) {
              case 'l': //Front left motor
                motorFrontLeft.writeMicroseconds(1100);
                Serial.println(F("Front left motor on"));
                while(Serial.read() != 's'); //Off
                motorFrontLeft.writeMicroseconds(1000);
                Serial.println(F("Front left motor off"));
                break;
              case 'r': //Front right motor
                motorFrontRight.writeMicroseconds(1100);
                Serial.println(F("Front right motor on"));
                while(Serial.read() != 's'); //Off
                motorFrontRight.writeMicroseconds(1000);
                Serial.println(F("Front right motor off"));
                break;
              default:
                Serial.println(F("Motor name not recognised"));
                break;
            }
            break;
          case 'b': //Back motor
            switch (command.charAt(2)) {
              case 'l': //Back left motor
                motorBackLeft.writeMicroseconds(1100);
                Serial.println(F("Back left motor on"));
                while(Serial.read() != 's'); //Off
                motorBackLeft.writeMicroseconds(1000);
                Serial.println(F("Back left motor off"));
                break;
              case 'r': //Back right motor
                motorBackRight.writeMicroseconds(1100);
                Serial.println(F("Back right motor on"));
                while(Serial.read() != 's'); //Off
                motorBackRight.writeMicroseconds(1000);
                Serial.println(F("Back right motor off"));
                break;
              default:
                Serial.println(F("Motor name not recognised"));
                break;
            }
            break;
          default:
            Serial.println(F("Motor name not recognised"));
            break;
        }
        break;
      default:
        Serial.println(F("Command not recognised"));
        break;
    }
  }
}

//END PROGRAM

//Copyright (C) 2016 D. Falck
//Licensed under the GNU General Public License version 3 or later. See header section for more information.

Well, that’s about it. I spent a while trying to tune the PID constants (and had a few unfortunate crashes resulting in the reprinting of arms) and called it a day. Perhaps in the future I’ll rebuild it (or make something similar) and persist to actually make it fly well, but at the time I had much else to worry about so I submitted it as it was.

Here’s an annotated render of the centre of the completed CAD design, with the battery lid off:

CentreRender1

And here’s the full render of the entire, closed, quad:

EntireQuadRender2

Now photos. Here’s the whole finished thing:

QuadPicture1

You can see the battery fits very nicely on top, with the low voltage alarm stuck on with Velcro.

Well, that’s about it. Here’s an annotated engineering drawing of the whole thing:

Entire Quad 1 Drawing v2

And in the next post I’ll round this all off by uploading the files used, so that anyone can take this project and develop it further. But remember the license info (in the footer of this blog) – please credit me!

 

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *