Quadrotor

/*

  AeroQuad v2.1 - October 2010

  www.AeroQuad.com

  Copyright (c) 2010 Ted Carancho.  All rights reserved.

  An Open Source Arduino based multicopter.

  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. 

  You should have received a copy of the GNU General Public License 

  along with this program. If not, see <http://www.gnu.org/licenses/>. 

*/

/****************************************************************************

   Before flight, select the different user options for your AeroQuad below

   Also, consult the ReadMe.mht file for additional details

   If you need additional assitance go to http://AeroQuad.com/forum

*****************************************************************************/

/****************************************************************************

 ************************* Hardware Configuration ***************************

 ****************************************************************************/

// Select which hardware you wish to use with the AeroQuad Flight Software

//#define AeroQuad_v1         // Arduino 2009 with AeroQuad Shield v1.7 and below

//#define AeroQuad_v18        // Arduino 2009 with AeroQuad Shield v1.8

//#define AeroQuad_Wii        // Arduino 2009 with Wii Sensors and AeroQuad Shield v1.x

//#define AeroQuadMega_v1     // Arduino Mega with AeroQuad Shield v1.7 and below

#define AeroQuadMega_v2     // Arduino Mega with AeroQuad Shield v2.x

//#define AeroQuadMega_Wii    // Arduino Mega with Wii Sensors and AeroQuad Shield v2.x

//#define ArduCopter          // ArduPilot Mega (APM) with APM Sensor Board

//#define Multipilot          // Multipilot board with Lys344 and ADXL 610 Gyro (needs debug)

//#define MultipilotI2C       // Active Multipilot I2C and Mixertable (needs debug)

/****************************************************************************

 *********************** Define Flight Configuration ************************

 ****************************************************************************/

// Use only one of the following definitions

#define plusConfig

//#define XConfig

//#define HEXACOAXIAL

//#define HEXARADIAL

// 5DOF IMU Version

// Uncomment this if you have the version of the 5DOF IMU which uses the older IDG300 or IDG500 gyros

//#define OriginalIMU 

// Yaw Gyro Type

// Use only one of the following definitions

#define IXZ // IXZ-500 Flat Yaw Gyro or ITG-3200 Triple Axis Gyro

//#define IDG // IDG-300 or IDG-500 Dual Axis Gyro

// Camera Stabilization (experimental)

// Not yet fully tested and implemented

//#define Camera

#define GPS

// Optional Sensors

#define HeadingMagHold // Enables HMC5843 Magnetometer

#define AltitudeHold // Enables BMP083 Barometer

#define SonarHold // Enables SRF02 Sonar

#define DebugPort Serial

/****************************************************************************

 ********************* End of User Definition Section ***********************

 ****************************************************************************/

#include <EEPROM.h>

//#include <Servo.h>

#include <Wire.h>

#include "AeroQuad.h"

#include "I2C.h"

#include "PID.h"

#include "Filter.h"

#include "Receiver.h"

#include "DataAcquisition.h"

#include "Accel.h"

#include "Gyro.h"

#include "Motors.h"

// Create objects defined from Configuration Section above

#ifdef AeroQuad_v1 

  Accel_AeroQuad_v1 accel;

  Gyro_AeroQuad_v1 gyro;

  Receiver_AeroQuad receiver;

  Motors_PWM motors;

  #include "FlightAngle.h"

  FlightAngle_CompFilter flightAngle;

  //FlightAngle_MultiWii flightAngle;

#endif

#ifdef AeroQuad_v18

  Accel_AeroQuadMega_v2 accel;

  Gyro_AeroQuadMega_v2 gyro;

  Receiver_AeroQuad receiver;

  Motors_PWM motors;

  #include "FlightAngle.h"

  FlightAngle_CompFilter flightAngle;

  //FlightAngle_DCM flightAngle;

#endif

#ifdef AeroQuadMega_v1

  // Special thanks to Wilafau for fixes for this setup

  // http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11466&viewfull=1#post11466

  Receiver_AeroQuadMega receiver;

  Accel_AeroQuad_v1 accel;

  Gyro_AeroQuad_v1 gyro;

  Motors_PWM motors;

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

#ifdef AeroQuadMega_v2

  Receiver_AeroQuadMega receiver;

  Motors_PWM motors;

  Accel_AeroQuadMega_v2 accel;

  Gyro_AeroQuadMega_v2 gyro;

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

#ifdef ArduCopter

  Gyro_ArduCopter gyro;

  Accel_ArduCopter accel;

  Receiver_ArduCopter receiver;

  Motors_ArduCopter motors;

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

#ifdef AeroQuad_Wii

  Accel_Wii accel;

  Gyro_Wii gyro;

  Receiver_AeroQuad receiver;

  Motors_PWM motors;

  #include "FlightAngle.h"

  FlightAngle_CompFilter flightAngle;

  //FlightAngle_DCM flightAngle;

#endif

#ifdef AeroQuadMega_Wii

  Accel_Wii accel;

  Gyro_Wii gyro;

  Receiver_AeroQuadMega receiver;

  Motors_PWM motors;

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

#ifdef Multipilot

  Accel_AeroQuad_v1 accel;

  Gyro_AeroQuad_v1 gyro;

  Receiver_Multipilot receiver;

  Motors_PWM motors;

  //#define PRINT_MIXERTABLE

  //#define TELEMETRY_DEBUG

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

#ifdef MultipilotI2C  

  Accel_AeroQuad_v1 accel;

  Gyro_AeroQuad_v1 gyro;

  Receiver_Multipilot receiver;

  Motors_I2C motors;

  //#define PRINT_MIXERTABLE

  //#define TELEMETRY_DEBUG

  #include "FlightAngle.h"

  FlightAngle_DCM flightAngle;

#endif

// Optional Sensors, currently defined for the AeroQuad v2.0 Shield

// Defined here in case other configurations want to incorporate these sensors

#ifdef HeadingMagHold

  #include "Compass.h"

  Compass_AeroQuad_v2 compass; // HMC5843

#endif

#ifdef AltitudeHold

  #include "Altitude.h"

  Altitude_AeroQuad_v2 altitude;  //BMP085

#endif

#ifdef SonarHold

  #include "Sonar.h"

  Sonar sonar;  //SRF02

#endif

#ifdef GPS

  #include "GPS.h"

  #include "TinyGPS.h"

  TinyGPS myGPS;

#endif

// Include this last as it contains objects from above declarations

#include "DataStorage.h"

// Angle Estimation Objects

// Class definition for angle estimation found in FlightAngle.h

// Use only one of the following variable declarations

// Insert into the appropriate #ifdef's above

//#include "FlightAngle.h"

//FlightAngle_CompFilter flightAngle; // Use this for Complementary Filter

//FlightAngle_KalmanFilter flightAngle; // Use this for Kalman Filter

//FlightAngle_IMU flightAngle; // Use this for IMU filter (do not use, for experimentation only)

//FlightAngle_MultiWii flightAngle;

// DCM gyro null values are defined in flightAngle.initalize()

// Review those values is you add a new #define which uses FlightAngle_DCM

//FlightAngle_DCM flightAngle; // Use this for DCM (only for Arduino Mega)

// ************************************************************

// ********************** Setup AeroQuad **********************

// ************************************************************

void setup() {

  DebugPort.begin(BAUD);

  pinMode(LEDPIN, OUTPUT);

  digitalWrite(LEDPIN, LOW);

  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)

    pinMode(LED2PIN, OUTPUT);

    digitalWrite(LED2PIN, LOW);

    pinMode(LED3PIN, OUTPUT);

    digitalWrite(LED3PIN, LOW);

  #endif

  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) || defined(AeroQuad_Wii) || defined(AeroQuadMega_Wii)

    Wire.begin();

  #endif

  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)

    // Recommendation from Mgros to increase I2C speed

    // http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11262&viewfull=1#post11262

    TWBR = 12;

  #endif

  // Read user values from EEPROM

  readEEPROM(); // defined in DataStorage.h

  // Configure motors

  motors.initialize(); // defined in Motors.h

  // Setup receiver pins for pin change interrupts

  if (receiverLoop == ON)

    receiver.initialize(); // defined in Received.h

  // Initialize sensors

  // If sensors have a common initialization routine

  // insert it into the gyro class because it executes first

  gyro.initialize(); // defined in Gyro.h

  accel.initialize(); // defined in Accel.h

  // Calibrate sensors

  gyro.autoZero(); // defined in Gyro.h

  zeroIntegralError();

  levelAdjust[ROLL] = 0;

  levelAdjust[PITCH] = 0;

  // Setup correct sensor orientation

  #ifdef AeroQuad_v1

    gyro.invert(PITCH);

    gyro.invert(ROLL);

  #endif 

  #ifdef AeroQuadMega_v1

    gyro.invert(PITCH);

    gyro.invert(ROLL);

  #endif 

  #ifdef OriginalIMU

    gyro.invert(PITCH);

    gyro.invert(ROLL);

  #endif

  #ifdef IXZ

    gyro.invert(YAW);

  #endif

  #ifdef ArduCopter

    gyro.invert(YAW);

    gyro.invert(PITCH);

    gyro.invert(ROLL);

  #endif

  #ifdef AeroQuad_Wii

    accel.invert(ROLL);

    gyro.invert(PITCH);

    gyro.invert(YAW);

  #endif

  #ifdef AeroQuadMega_Wii

    accel.invert(ROLL);

    gyro.invert(PITCH);

    gyro.invert(YAW);

  #endif

  #ifdef Multipilot

    accel.invert(PITCH);

    gyro.invert(ROLL);

  #endif

  // Flight angle estimiation

  flightAngle.initialize(); // defined in FlightAngle.h

  // Optional Sensors

  #ifdef HeadingMagHold

    compass.initialize();

    setHeading = compass.getHeading();

  #endif

  #ifdef AltitudeHold

    altitude.initialize();

  #endif

  #ifdef SonarHold

    sonar.initialize(0x70);

  #endif

  // Camera stabilization setup

  #ifdef Camera

    rollCamera.attach(ROLLCAMERAPIN);

    pitchCamera.attach(PITCHCAMERAPIN);

  #endif

  previousTime = millis();

  digitalWrite(LEDPIN, HIGH);

  safetyCheck = 0;

}

// ************************************************************

// ******************** Main AeroQuad Loop ********************

// ************************************************************

void loop () {

  // Measure loop rate

  currentTime = millis();

  deltaTime = currentTime - previousTime;

  G_Dt = deltaTime / 1000.0;

  previousTime = currentTime;

  #ifdef DEBUG

    if (testSignal == LOW) testSignal = HIGH;

    else testSignal = LOW;

    digitalWrite(LEDPIN, testSignal);

  #endif

  // show what we're reading from GPS

  Serial.print("Show GPS info: \n\n");

  gpsdump(myGPS);

  // Reads external pilot commands and performs functions based on stick configuration

  if ((currentTime > (receiverTime + RECEIVERLOOPTIME)) && (receiverLoop == ON)) {// 10Hz

    readPilotCommands(); // defined in FlightCommand.pde

    receiverTime = currentTime;

  }

  // Measures sensor data and calculates attitude

  if ((currentTime > (sensorTime + AILOOPTIME)) && (sensorLoop == ON)) { // 500Hz

    readSensors(); // defined in Sensors.pde

    sensorTime = currentTime;

  } 

  // Combines external pilot commands and measured sensor data to generate motor commands

  if ((currentTime > controlLoopTime + CONTROLLOOPTIME) && (controlLoop == ON)) { // 500Hz

    flightControl(); // defined in FlightControl.pde

    controlLoopTime = currentTime;

  } 

  // Listen for configuration commands and reports telemetry

  if ((currentTime > telemetryTime + TELEMETRYLOOPTIME) && (telemetryLoop == ON)) { // 10Hz    

    readSerialCommand(); // defined in SerialCom.pde

    sendSerialTelemetry(); // defined in SerialCom.pde

    telemetryTime = currentTime;

  }

#ifdef Camera // Experimental, not fully implemented yet

  // Command camera stabilization servos (requires #include <servo.h>)

  if ((currentTime > (cameraTime + CAMERALOOPTIME)) && (cameraLoop == ON)) { // 50Hz

    rollCamera.write((mCamera * flightAngle.get(ROLL)) + bCamera);

    pitchCamera.write((mCamera * flightAngle.get(PITCH)) + bCamera);

    cameraTime = currentTime;

  }

#endif

}