Custom Quadcopter

Archive
1

Hello!

I have made my own all PCB quadcopter, of which I have attached an image, and I am wondering what exactly I need to do to get the Ardupilot working with my device. In terms of hardware, there are not very many variations, other than the fact that:

  1. It does not use an ESC, just a PWM signal to a MOSFET.
  2. There is no RC, it, for now, must just hover level.

Basically I want to solely stabilise the platform. Here is my arduino code so far, I started off writing from scratch:

[code]

// ================================================================
// === Referenc Material ===
// ================================================================

// http://theboredengineers.com/2012/05/the-quadcopter-basics/
// http://blog.oscarliang.net/quadcopter-pid-explained-tuning/
// https://github.com/grantmd/QuadCopter
// http://www.pdx.edu/nanogroup/sites/www.pdx.edu.nanogroup/files/2013_Arduino%20PID%20Lab_0.pdf
// http://www.maelabs.ucsd.edu/mae156alib/control/PID-Control-Ardunio.pdf
// ================================================================
// === LIBRARY INCLUSIONS ===
// ================================================================
#include <Wire.h> // I2C Library
#include “I2Cdev.h” // Advanced I2C Functions
#include “MPU9150Lib.h” // MPU-9150 Motion Processing Fuctions
#include “CalLib.h” // MPU-9150 Calibration Algorithms
#include <dmpKey.h> // MPU-9150 DMP Register Keys
#include <dmpmap.h> // MPU-9150 DMP Register Map
#include <inv_mpu.h> // MPU-9150 Low Level Motion Processing Algorithms
#include <inv_mpu_dmp_motion_driver.h> // MPU-9150 DMP Algorithms
#include <EEPROM.h> // EEPROM Controller
#include <PID_v1.h>

// ================================================================
// === MOTOR DEFINITIONS ===
// ================================================================
#define MOTOR_FL 4 // Front left CCW
#define MOTOR_FR 5 // Front right CW
#define MOTOR_BL 8 // back left CW
#define MOTOR_BR 3 // back right CCW

// ================================================================
// === CLASS DECLARATIONS ===
// ================================================================
MPU9150Lib MPU;

// ================================================================
// === CONSTANT DECLARATIONS ===
// ================================================================
float gRes = 2000.0/32768.0; // Gyro Scale Resolution

// ================================================================
// === MPU1950 VARIABLE DECLARATIONS ===
// ================================================================
#define DEVICE_TO_USE 0 // 0 = 0x68, 1 = 0x69
#define MPU_UPDATE_RATE (20) // MPU_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the sensor data and DMP output
#define MAG_UPDATE_RATE (15) // MAG_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the magnetometer data, should be less than or equal to MPU_UPDATE_RATE
#define MPU_MAG_MIX_GYRO_AND_SOME_MAG 10 // MPU_MAG_MIX defines the influence that the magnetometer has on the yaw output (0 = Only Gyro, 1 = Only Mag, 50 = Mainly Gyros with Mag)
#define MPU_LPF_RATE 40 // MPU_LPF_RATE is the low pas filter rate and can be between 5 and 188Hz

// ================================================================
// === KINEMATICS ===
// ================================================================

// Current Orientation Data
double currentRoll = 0.0;
double currentPitch = 0.0;
double currentHeading = 0.0;

// Current Gyro Data
double gyroRateX;
double gyroRateY;
double gyroRateZ;

// Target Gyro Data - Generated by PID
double targetgyroRateX;
double targetgyroRateY;
double targetgyroRateZ;

// Target Orientation - Assuming We Want to be Level
double targetPitch = 0.0;
double targetRoll = 0.0;
double targetHeading = 0.0;

// Initial Throttle Value
float biasThrottle = 220;

// Throttle Adjust Values
double pitchAdjust;
double rollAdjust;

// PID Objects
PID pitchStabPID(&currentPitch, &targetgyroRateX, &targetPitch,(double)1,(double)0,(double)0, DIRECT);
PID pitchRatePID(&gyroRateX, &pitchAdjust, &targetgyroRateX,(double)0.15,(double)0,(double)0, DIRECT);
PID rollStabPID(&currentRoll, &targetgyroRateY, &targetRoll,(double)1,(double)0,(double)0, DIRECT);
PID rollRatePID(&gyroRateY, &rollAdjust, &targetgyroRateY,(double)0.15,(double)0,(double)0, DIRECT);

// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {

// Pin Mode Declarations
pinMode(MOTOR_FL, OUTPUT);
pinMode(MOTOR_FR, OUTPUT);
pinMode(MOTOR_BL, OUTPUT);
pinMode(MOTOR_BR, OUTPUT);

// Pour a Bowl of Serial
Serial.begin(115200);
Serial.println(“Serial link established…”);

// Initialize I2C bus
Wire.begin();

// Select IMU address & Initialise IMU with Calibration Data & DMP Functionality
MPU.selectDevice(DEVICE_TO_USE);
MPU.init(MPU_UPDATE_RATE, MPU_MAG_MIX_GYRO_AND_SOME_MAG, MAG_UPDATE_RATE, MPU_LPF_RATE);

// Setup PID Controllers
PIDInit();

// Start Motors at Idle Speed ~ 20%
engineStart();

}

// ================================================================
// === MAIN LOOP ===
// ================================================================

void loop() {

// DMP Complete
if (MPU.read()){      
  // Calculate Orientation in Degrees     
  currentPitch = MPU.m_fusedEulerPose[VEC3_X] * RAD_TO_DEGREE;
  currentRoll = MPU.m_fusedEulerPose[VEC3_Y] * RAD_TO_DEGREE;
  currentHeading = MPU.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE;

 // Calculate Rotationial Rate in Degrees per Sec
 gyroRateX= MPU.m_rawGyro[VEC3_X] * gRes;  // Pitch Rate
 gyroRateY= MPU.m_rawGyro[VEC3_Y] * -gRes;  // Roll Rate
 gyroRateZ= MPU.m_rawGyro[VEC3_Z] * gRes;
 
 /*Serial.print("Pitch:      ");
 Serial.print(currentHeading);
 Serial.print("       Rate:      ");
 Serial.println(gyroRateZ);*/
  
}

  // ================================================================
  // ===                    PID Algorithm Here!                   ===
  // ================================================================

  // This is where the Black Magic happens....
  pitchStabPID.Compute();
  rollStabPID.Compute();  
  pitchRatePID.Compute();
  rollRatePID.Compute(); 
  
  // Debug!
  /*Serial.print("Pitch: ");
  Serial.print(currentPitch);
  Serial.print("    ");
  Serial.print("Target Rate: ");
  Serial.print(targetgyroRateX);
  Serial.print("    ");
  Serial.print("Actual Rate: ");
  Serial.print(gyroRateX);
  Serial.print("    ");
  Serial.print("Adjust: ");
  Serial.println(pitchAdjust);
  Serial.println();*/
  
  Serial.print("Pitch:      "  );
  Serial.print(pitchAdjust);
  Serial.print("      Roll:      "  );
  Serial.println(rollAdjust);
  
  // Motor Offset Fusion
  /*analogWrite(MOTOR_FL, biasThrottle + pitchAdjust);
  analogWrite(MOTOR_FR, biasThrottle + pitchAdjust);
  analogWrite(MOTOR_BL, biasThrottle - pitchAdjust);
  analogWrite(MOTOR_BR, biasThrottle - pitchAdjust);*/    

  pitchAdjust = (1.5 * pitchAdjust);
  rollAdjust = (1.5 * rollAdjust);
  
  analogWrite(MOTOR_FL, biasThrottle + pitchAdjust - rollAdjust);
  analogWrite(MOTOR_FR, biasThrottle + pitchAdjust + rollAdjust);
  analogWrite(MOTOR_BL, biasThrottle - pitchAdjust - rollAdjust);
  analogWrite(MOTOR_BR, biasThrottle - pitchAdjust + rollAdjust);

}

// ================================================================
// === PID Initilisation ===
// ================================================================
void PIDInit(){

pitchStabPID.SetSampleTime(15);
pitchStabPID.SetMode(AUTOMATIC);
pitchStabPID.SetOutputLimits(-50, 50);

pitchRatePID.SetSampleTime(15);
pitchRatePID.SetMode(AUTOMATIC);
pitchRatePID.SetOutputLimits(-75, 75);

rollStabPID.SetSampleTime(15);
rollStabPID.SetMode(AUTOMATIC);
rollStabPID.SetOutputLimits(-50, 50);

rollRatePID.SetSampleTime(15);
rollRatePID.SetMode(AUTOMATIC);
rollRatePID.SetOutputLimits(-75, 75);
}

// ================================================================
// === MOTOR CONTROL ROUTINES ===
// ================================================================

void engineStart(){

Serial.print("Begin Engine Startup… ");

// Idle Motors at 20%
analogWrite(MOTOR_FL, 55);
delay (800);
analogWrite(MOTOR_FR, 55);
delay (800);
analogWrite(MOTOR_BL, 55);
delay (800);
analogWrite(MOTOR_BR, 55);
delay(800);

// All Good If Were Here!
Serial.println(“Engines Active”);
}

void engineKill(){

Serial.println(“Engine Cutout…”);
// Kill all Motors
analogWrite(MOTOR_FL, 1);
analogWrite(MOTOR_FR, 1);
analogWrite(MOTOR_BL, 1);
analogWrite(MOTOR_BR, 1);

}[/code]

2

I can’t answer any of your questions, but I was just wondering on how you like the MPU1950, specifically compared to the MPU6050. I haven’t had a chance to look through your code yet, but I was wondering if you have integrated the compass on controlling the yaw? Is it worth using this MPU over the 6050? Thank you, and good luck with your build.