From 31bf5a1f5da07b1eb3a06d95be7d893e31a32119 Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Mon, 18 Apr 2011 02:13:57 +0000 Subject: [PATCH] removed legacy_do_not_use directory and contents as it's just messy to have it hanging around. older versions can be retrieved from SVN if need be. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1904 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- legacy_do_not_use/APM_Compass/APM_Compass.cpp | 359 -------- legacy_do_not_use/APM_Compass/APM_Compass.h | 113 --- .../APM_Compass_test/APM_Compass_test.pde | 88 -- legacy_do_not_use/APM_Compass/keywords.txt | 19 - legacy_do_not_use/Arducopter/ArduCopter.h | 253 ------ legacy_do_not_use/Arducopter/Arducopter.pde | 821 ------------------ legacy_do_not_use/Arducopter/DCM.pde | 247 ------ legacy_do_not_use/Arducopter/Functions.pde | 151 ---- legacy_do_not_use/Arducopter/Log.pde | 267 ------ legacy_do_not_use/Arducopter/Navigation.pde | 73 -- legacy_do_not_use/Arducopter/Sensors.pde | 72 -- legacy_do_not_use/Arducopter/SerialCom.pde | 395 --------- legacy_do_not_use/Arducopter/UserConfig.h | 399 --------- 13 files changed, 3257 deletions(-) delete mode 100644 legacy_do_not_use/APM_Compass/APM_Compass.cpp delete mode 100644 legacy_do_not_use/APM_Compass/APM_Compass.h delete mode 100644 legacy_do_not_use/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde delete mode 100644 legacy_do_not_use/APM_Compass/keywords.txt delete mode 100644 legacy_do_not_use/Arducopter/ArduCopter.h delete mode 100644 legacy_do_not_use/Arducopter/Arducopter.pde delete mode 100644 legacy_do_not_use/Arducopter/DCM.pde delete mode 100644 legacy_do_not_use/Arducopter/Functions.pde delete mode 100644 legacy_do_not_use/Arducopter/Log.pde delete mode 100644 legacy_do_not_use/Arducopter/Navigation.pde delete mode 100644 legacy_do_not_use/Arducopter/Sensors.pde delete mode 100644 legacy_do_not_use/Arducopter/SerialCom.pde delete mode 100644 legacy_do_not_use/Arducopter/UserConfig.h diff --git a/legacy_do_not_use/APM_Compass/APM_Compass.cpp b/legacy_do_not_use/APM_Compass/APM_Compass.cpp deleted file mode 100644 index 2baf20c913..0000000000 --- a/legacy_do_not_use/APM_Compass/APM_Compass.cpp +++ /dev/null @@ -1,359 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - APM_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Sensor is conected to I2C port - Sensor is initialized in Continuos mode (10Hz) - - Variables: - Heading : Magnetic heading - Heading_X : Magnetic heading X component - Heading_Y : Magnetic heading Y component - Mag_X : Raw X axis magnetometer data - Mag_Y : Raw Y axis magnetometer data - Mag_Z : Raw Z axis magnetometer data - lastUpdate : the time of the last successful reading - - Methods: - Init() : Initialization of I2C and sensor - Read() : Read Sensor data - Calculate(float roll, float pitch) : Calculate tilt adjusted heading - SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass - SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances - SetDeclination(float radians) : Set heading adjustment between true north and magnetic north - - To do : code optimization - Mount position : UPDATED - Big capacitor pointing backward, connector forward - -*/ -extern "C" { - // AVR LibC Includes - #include - #include "WConstants.h" -} - -#include -#include "APM_Compass.h" - -#define CompassAddress 0x1E -#define ConfigRegA 0x00 -#define ConfigRegB 0x01 -#define MagGain 0x20 -#define PositiveBiasConfig 0x11 -#define NegativeBiasConfig 0x12 -#define NormalOperation 0x10 -#define ModeRegister 0x02 -#define ContinuousConversion 0x00 -#define SingleConversion 0x01 - -// Constructors //////////////////////////////////////////////////////////////// -APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0) -{ - // mag x y z offset initialisation - offset[0] = 0; - offset[1] = 0; - offset[2] = 0; - - // initialise orientation matrix - orientationMatrix = ROTATION_NONE; -} - -// Public Methods ////////////////////////////////////////////////////////////// -bool APM_Compass_Class::Init(int initialiseWireLib) -{ - unsigned long currentTime = millis(); // record current time - int numAttempts = 0; - int success = 0; - - if( initialiseWireLib != 0 ) - Wire.begin(); - - delay(10); - - // calibration initialisation - calibration[0] = 1.0; - calibration[1] = 1.0; - calibration[2] = 1.0; - - while( success == 0 && numAttempts < 5 ) - { - // record number of attempts at initialisation - numAttempts++; - - // force positiveBias (compass should return 715 for all channels) - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegA); - Wire.send(PositiveBiasConfig); - if (0 != Wire.endTransmission()) - continue; // compass not responding on the bus - delay(50); - - // set gains - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegB); - Wire.send(MagGain); - Wire.endTransmission(); - delay(10); - - Wire.beginTransmission(CompassAddress); - Wire.send(ModeRegister); - Wire.send(SingleConversion); - Wire.endTransmission(); - delay(10); - - // read values from the compass - Read(); - delay(10); - - // calibrate - if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000) - { - calibration[0] = fabs(715.0 / Mag_X); - calibration[1] = fabs(715.0 / Mag_Y); - calibration[2] = fabs(715.0 / Mag_Z); - - // mark success - success = 1; - } - - // leave test mode - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegA); - Wire.send(NormalOperation); - Wire.endTransmission(); - delay(50); - - Wire.beginTransmission(CompassAddress); - Wire.send(ModeRegister); - Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) - Wire.endTransmission(); // End transmission - delay(50); - } - return(success); -} - -// Read Sensor data -void APM_Compass_Class::Read() -{ - int i = 0; - byte buff[6]; - - Wire.beginTransmission(CompassAddress); - Wire.send(0x03); //sends address to read from - Wire.endTransmission(); //end transmission - - //Wire.beginTransmission(CompassAddress); - Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device - while(Wire.available()) - { - buff[i] = Wire.receive(); // receive one byte - i++; - } - Wire.endTransmission(); //end transmission - - if (i==6) // All bytes received? - { - // MSB byte first, then LSB, X,Y,Z - Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis - Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis - Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis - lastUpdate = millis(); // record time of update - } -} - -void APM_Compass_Class::Calculate(float roll, float pitch) -{ - float Head_X; - float Head_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - Vector3f rotMagVec; - - cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // rotate the magnetometer values depending upon orientation - if( orientation == 0 ) - rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - else - rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - - // Tilt compensated Magnetic field X component: - Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; - // Tilt compensated Magnetic field Y component: - Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; - // Magnetic Heading - Heading = atan2(-Head_Y,Head_X); - - // Declination correction (if supplied) - if( declination != 0.0 ) - { - Heading = Heading + declination; - if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) - Heading -= (2.0 * M_PI); - else if (Heading < -M_PI) - Heading += (2.0 * M_PI); - } - - // Optimization for external DCM use. Calculate normalized components - Heading_X = cos(Heading); - Heading_Y = sin(Heading); -} - -void APM_Compass_Class::SetOrientation(const Matrix3f &rotationMatrix) -{ - orientationMatrix = rotationMatrix; - if( orientationMatrix == ROTATION_NONE ) - orientation = 0; - else - orientation = 1; -} - -void APM_Compass_Class::SetOffsets(int x, int y, int z) -{ - offset[0] = x; - offset[1] = y; - offset[2] = z; -} - -void APM_Compass_Class::SetDeclination(float radians) -{ - declination = radians; -} - - -// Constructors //////////////////////////////////////////////////////////////// -APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0) -{ - // mag x y z offset initialisation - offset[0] = 0; - offset[1] = 0; - offset[2] = 0; - - // initialise orientation matrix - orientationMatrix = ROTATION_NONE; -} - -// Public Methods ////////////////////////////////////////////////////////////// -bool APM_Compass_HIL_Class::Init(int initialiseWireLib) -{ - unsigned long currentTime = millis(); // record current time - int numAttempts = 0; - int success = 0; - - // calibration initialisation - calibration[0] = 1.0; - calibration[1] = 1.0; - calibration[2] = 1.0; - - while( success == 0 && numAttempts < 5 ) - { - // record number of attempts at initialisation - numAttempts++; - - // read values from the compass - Read(); - delay(10); - - // calibrate - if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000) - { - calibration[0] = fabs(715.0 / Mag_X); - calibration[1] = fabs(715.0 / Mag_Y); - calibration[2] = fabs(715.0 / Mag_Z); - - // mark success - success = 1; - } - } - return(success); -} - -// Read Sensor data -void APM_Compass_HIL_Class::Read() -{ - // values set by setHIL function -} - -void APM_Compass_HIL_Class::Calculate(float roll, float pitch) -{ - float Head_X; - float Head_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - Vector3f rotMagVec; - - cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // rotate the magnetometer values depending upon orientation - if( orientation == 0 ) - rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - else - rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - - // Tilt compensated Magnetic field X component: - Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; - // Tilt compensated Magnetic field Y component: - Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; - // Magnetic Heading - Heading = atan2(-Head_Y,Head_X); - - // Declination correction (if supplied) - if( declination != 0.0 ) - { - Heading = Heading + declination; - if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) - Heading -= (2.0 * M_PI); - else if (Heading < -M_PI) - Heading += (2.0 * M_PI); - } - - // Optimization for external DCM use. Calculate normalized components - Heading_X = cos(Heading); - Heading_Y = sin(Heading); -} - -void APM_Compass_HIL_Class::SetOrientation(const Matrix3f &rotationMatrix) -{ - orientationMatrix = rotationMatrix; - if( orientationMatrix == ROTATION_NONE ) - orientation = 0; - else - orientation = 1; -} - -void APM_Compass_HIL_Class::SetOffsets(int x, int y, int z) -{ - offset[0] = x; - offset[1] = y; - offset[2] = z; -} - -void APM_Compass_HIL_Class::SetDeclination(float radians) -{ - declination = radians; -} - -void APM_Compass_HIL_Class::setHIL(float _Mag_X, float _Mag_Y, float _Mag_Z) -{ - // TODO: map floats to raw - Mag_X = _Mag_X; - Mag_Y = _Mag_Y; - Mag_Z = _Mag_Z; -} diff --git a/legacy_do_not_use/APM_Compass/APM_Compass.h b/legacy_do_not_use/APM_Compass/APM_Compass.h deleted file mode 100644 index 876ec93d2a..0000000000 --- a/legacy_do_not_use/APM_Compass/APM_Compass.h +++ /dev/null @@ -1,113 +0,0 @@ -#ifndef APM_Compass_h -#define APM_Compass_h - -#include "../AP_Math/AP_Math.h" - -// Rotation matrices -#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) - -// orientations for DIYDrones magnetometer -#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE -#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45 -#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 -#define APM_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135 -#define APM_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 -#define APM_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225 -#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 -#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 - -// orientations for Sparkfun magnetometer -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180 -#define APM_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180 -#define APM_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45 - -class APM_Compass_Class -{ - private: - int orientation; - Matrix3f orientationMatrix; - float calibration[3]; - int offset[3]; - float declination; - public: - int Mag_X; - int Mag_Y; - int Mag_Z; - float Heading; - float Heading_X; - float Heading_Y; - unsigned long lastUpdate; - - APM_Compass_Class(); // Constructor - bool Init(int initialiseWireLib = 1); - void Read(); - void Calculate(float roll, float pitch); - void SetOrientation(const Matrix3f &rotationMatrix); - void SetOffsets(int x, int y, int z); - void SetDeclination(float radians); -}; - -class APM_Compass_HIL_Class -{ - private: - int orientation; - Matrix3f orientationMatrix; - float calibration[3]; - int offset[3]; - float declination; - public: - int Mag_X; - int Mag_Y; - int Mag_Z; - float Heading; - float Heading_X; - float Heading_Y; - unsigned long lastUpdate; - - APM_Compass_HIL_Class(); // Constructor - bool Init(int initialiseWireLib = 1); - void Read(); - void Calculate(float roll, float pitch); - void SetOrientation(const Matrix3f &rotationMatrix); - void SetOffsets(int x, int y, int z); - void SetDeclination(float radians); - void setHIL(float Mag_X, float Mag_Y, float Mag_Z); -}; - -#endif diff --git a/legacy_do_not_use/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde b/legacy_do_not_use/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde deleted file mode 100644 index 50ab6bde73..0000000000 --- a/legacy_do_not_use/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde +++ /dev/null @@ -1,88 +0,0 @@ -/* - Example of APM_Compass library (HMC5843 sensor). - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - offsets displayed are calculated from the min/max values from each axis. - rotate the compass so as to produce the maximum and minimum values - after the offsets stop changing, edit the code to pass these offsets into - APM_Compass.SetOffsets. -*/ - -#include -#include // Compass Library -#include // Math library - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -unsigned long timer; -APM_Compass_Class APM_Compass; - -void setup() -{ - Serial.begin(38400); - Serial.println("Compass library test (HMC5843)"); - - APM_Compass.Init(); // Initialization - APM_Compass.SetOrientation(APM_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft - APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference - APM_Compass.SetDeclination(ToRad(0.0)); // set local difference between magnetic north and true north - - delay(1000); - timer = millis(); -} - -void loop() -{ - static float min[3], max[3], offset[3]; - - if((millis()- timer) > 100) - { - timer = millis(); - APM_Compass.Read(); - APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example - - // capture min - if( APM_Compass.Mag_X < min[0] ) - min[0] = APM_Compass.Mag_X; - if( APM_Compass.Mag_Y < min[1] ) - min[1] = APM_Compass.Mag_Y; - if( APM_Compass.Mag_Z < min[2] ) - min[2] = APM_Compass.Mag_Z; - - // capture max - if( APM_Compass.Mag_X > max[0] ) - max[0] = APM_Compass.Mag_X; - if( APM_Compass.Mag_Y > max[1] ) - max[1] = APM_Compass.Mag_Y; - if( APM_Compass.Mag_Z > max[2] ) - max[2] = APM_Compass.Mag_Z; - - // calculate offsets - offset[0] = -(max[0]+min[0])/2; - offset[1] = -(max[1]+min[1])/2; - offset[2] = -(max[2]+min[2])/2; - - // display all to user - Serial.print("Heading:"); - Serial.print(ToDeg(APM_Compass.Heading)); - Serial.print(" ("); - Serial.print(APM_Compass.Mag_X); - Serial.print(","); - Serial.print(APM_Compass.Mag_Y); - Serial.print(","); - Serial.print(APM_Compass.Mag_Z); - Serial.print(")"); - - // display offsets - Serial.print("\t offsets("); - Serial.print(offset[0]); - Serial.print(","); - Serial.print(offset[1]); - Serial.print(","); - Serial.print(offset[2]); - Serial.print(")"); - - Serial.println(); - } -} diff --git a/legacy_do_not_use/APM_Compass/keywords.txt b/legacy_do_not_use/APM_Compass/keywords.txt deleted file mode 100644 index 4f4b3ec7ab..0000000000 --- a/legacy_do_not_use/APM_Compass/keywords.txt +++ /dev/null @@ -1,19 +0,0 @@ -Compass KEYWORD1 -AP_Compass KEYWORD1 -APM_Compass KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Calculate KEYWORD2 -SetOrientation KEYWORD2 -SetOffsets KEYWORDS2 -SetDeclination KEYWORDS2 -Heading KEYWORD2 -Heading_X KEYWORD2 -Heading_Y KEYWORD2 -Mag_X KEYWORD2 -Mag_Y KEYWORD2 -Mag_Z KEYWORD2 -lastUpdate KEYWORD2 - - - diff --git a/legacy_do_not_use/Arducopter/ArduCopter.h b/legacy_do_not_use/Arducopter/ArduCopter.h deleted file mode 100644 index 32b133071a..0000000000 --- a/legacy_do_not_use/Arducopter/ArduCopter.h +++ /dev/null @@ -1,253 +0,0 @@ -/* - ArduCopter 1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . -*/ - -#include "WProgram.h" - -/*******************************************************************/ -// ArduPilot Mega specific hardware and software settings -// -// DO NOT EDIT unless you are absolytely sure of your doings. -// User configurable settings are on UserConfig.h -/*******************************************************************/ - -/**************************************************************/ -// Special features that might disapear in future releases - -/* APM Hardware definitions */ -#define LED_Yellow 36 -#define LED_Red 35 -#define LED_Green 37 -#define RELE_pin 47 -#define SW1_pin 41 -#define SW2_pin 40 - -//#define VDIV1 AN1 -//#define VDIV2 AN2 -//#define VDIV3 AN3 -//#define VDIV4 AN4 - -//#define AN5 -//#define AN6 - -// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware - -// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ -int SENSOR_SIGN[]={ - 1, -1, -1, -1, 1, 1, -1, -1, -1}; - //{-1,1,-1,1,-1,1,-1,-1,-1}; -/* APM Hardware definitions, END */ - -/* General definitions */ - -#define TRUE 1 -#define FALSE 0 -#define ON 1 -#define OFF 0 - - -// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step -// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 -// Tested value : 408 -#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer -#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 -// Tested values : -#define Gyro_Gain_X 0.4 //X axis Gyro gain -#define Gyro_Gain_Y 0.41 //Y axis Gyro gain -#define Gyro_Gain_Z 0.41 //Z axis Gyro gain -#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second - -/*For debugging purposes*/ -#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data - -int AN[6]; //array that store the 6 ADC channels -int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers -int gyro_temp; - - -float G_Dt=0.02; // Integration time for the gyros (DCM algorithm) -float Accel_Vector[3]= {0, 0, 0}; //Store the acceleration in a vector -float Accel_Vector_unfiltered[3]= {0, 0, 0}; //Store the acceleration in a vector -float Gyro_Vector[3]= {0, 0, 0}; //Store the gyros rutn rate in a vector -float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data -float Omega_P[3]= {0, 0, 0}; //Omega Proportional correction -float Omega_I[3]= {0, 0, 0}; //Omega Integrator -float Omega[3]= {0, 0, 0}; -//float Accel_magnitude; -//float Accel_weight; - -float errorRollPitch[3] = {0, 0, 0}; -float errorYaw[3] = {0, 0, 0}; -float errorCourse = 0; -float COGX = 0; //Course overground X axis -float COGY = 1; //Course overground Y axis - -float roll = 0; -float pitch = 0; -float yaw = 0; - -unsigned int counter = 0; - -float DCM_Matrix[3][3]= { - { 1,0,0 }, - { 0,1,0 }, - { 0,0,1 }}; -float Update_Matrix[3][3]={ - { 0,1,2 }, - { 3,4,5 }, - { 6,7,8 }}; //Gyros here - -float Temporary_Matrix[3][3]={ - { 0,0,0 }, - { 0,0,0 }, - { 0,0,0 }}; - -// GPS variables -float speed_3d=0; -int GPS_ground_speed=0; - -// Main timers -long timer=0; -long timer_old; -long GPS_timer; -long GPS_timer_old; -float GPS_Dt=0.2; // GPS Dt - -// Attitude control variables -float command_rx_roll=0; // User commands -float command_rx_pitch=0; -float command_rx_yaw=0; -int control_roll; // PID control results -int control_pitch; -int control_yaw; -float K_aux; - -// Attitude PID controls -float roll_I=0; -float roll_D; -float err_roll; -float pitch_I=0; -float pitch_D; -float err_pitch; -float yaw_I=0; -float yaw_D; -float err_yaw; - -//Position control -long target_longitude; -long target_lattitude; -byte target_position; -float gps_err_roll; -float gps_err_roll_old; -float gps_roll_D; -float gps_roll_I=0; -float gps_err_pitch; -float gps_err_pitch_old; -float gps_pitch_D; -float gps_pitch_I=0; -float command_gps_roll; -float command_gps_pitch; - -//Altitude control -int Initial_Throttle; -int target_sonar_altitude; -int err_altitude; -int err_altitude_old; -float command_altitude; -float altitude_I; -float altitude_D; - -//Pressure Sensor variables -#ifdef UseBMP -unsigned long abs_press = 0; -unsigned long abs_press_filt = 0; -unsigned long abs_press_gnd = 0; -int ground_temperature = 0; // -int temp_unfilt = 0; -long ground_alt = 0; // Ground altitude from gps at startup in centimeters -long press_alt = 0; // Pressure altitude -#endif - - -#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO - -#define AIRSPEED_PIN 1 // Need to correct value -#define BATTERY_PIN 1 // Need to correct value -#define RELAY_PIN 47 -#define LOW_VOLTAGE 11.4 // Pack voltage at which to trigger alarm -#define INPUT_VOLTAGE 5.2 // (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course) -#define VOLT_DIV_RATIO 1.0 // Voltage divider ratio set with thru-hole resistor (see manual) - -float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter - - -// Sonar variables -int Sonar_value=0; -#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters -int Sonar_Counter=0; - -// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode) -byte AP_mode = 2; -byte FL_mode = 0; // Flight mode : 0 => Stable, 1 => Acro mode. Stable as default - - -// Mode LED timers and variables, used to blink LED_Green -byte gled_status = HIGH; -long gled_timer; -int gled_speed; - -long t0; -int num_iter; -float aux_debug; - -// Radio definitions -int roll_mid; -int pitch_mid; -int yaw_mid; - -int Neutro_yaw; -int ch_roll; -int ch_pitch; -int ch_throttle; -int ch_yaw; -int ch_aux; -int ch_aux2; - -int frontMotor; -int backMotor; -int leftMotor; -int rightMotor; -byte motorArmed = 0; -int minThrottle = 0; - -// Serial communication -char queryType; -long tlmTimer = 0; - -// Arming/Disarming -uint8_t Arming_counter=0; -uint8_t Disarming_counter=0; diff --git a/legacy_do_not_use/Arducopter/Arducopter.pde b/legacy_do_not_use/Arducopter/Arducopter.pde deleted file mode 100644 index 7d80de6116..0000000000 --- a/legacy_do_not_use/Arducopter/Arducopter.pde +++ /dev/null @@ -1,821 +0,0 @@ -/* ********************************************************************** */ -/* ArduCopter Quadcopter code */ -/* */ -/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ -/* IMU DCM code from Diydrones.com */ -/* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */ -/* Ardupilot core code : from DIYDrones.com development team */ -/* Authors : Arducopter development team */ -/* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */ -/* Jani Hirvinen, Ken McEwans, Roberto Navoni, */ -/* Sandro Benigno, Chris Anderson */ -/* Date : 08-08-2010 */ -/* Version : 1.3 beta */ -/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ -/* Mounting position : RC connectors pointing backwards */ -/* This code use this libraries : */ -/* APM_RC : Radio library (with InstantPWM) */ -/* APM_ADC : External ADC library */ -/* DataFlash : DataFlash log library */ -/* APM_BMP085 : BMP085 barometer library */ -/* APM_Compass : HMC5843 compass library [optional] */ -/* GPS_UBLOX or GPS_NMEA or GPS_MTK : GPS library [optional] */ -/* ********************************************************************** */ - -/* -**** Switch Functions ***** - AUX1 ON = Stable Mode - AUX1 OFF = Acro Mode - GEAR ON = GPS Hold - GEAR OFF = Flight Assist (Stable Mode) - - **** LED Feedback **** - Bootup Sequence: - 1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery - 2) A, B, C LED's have running light while calibrating Gyro/Acc's - 3) Green LED Solid after initialization finished - - Green LED On = APM Initialization Finished - Yellow LED On = GPS Hold Mode - Yellow LED Off = Flight Assist Mode (No GPS) - Red LED On = GPS Fix, 2D or 3D - Red LED Off = No GPS Fix - - Green LED blink slow = Motors armed, Stable mode - Green LED blink rapid = Motors armed, Acro mode - -*/ - -/* User definable modules */ - -// Comment out with // modules that you are not using -#define IsGPS // Do we have a GPS connected -//#define IsNEWMTEK// Do we have MTEK with new firmware -#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator -//#define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port -#define IsAM // Do we have motormount LED's. AM = Atraction Mode -#define AUTOMODE // New experimental Automode to change between Stable <=> Acro. If pitch/roll stick move is more than 50% change mode -//#define IsXBEE // Moves all serial communication to Telemetry port when activated. - -#define CONFIGURATOR // Do se use Configurator or normal text output over serial link - -/**********************************************/ -// Not in use yet, starting to work with battery monitors and pressure sensors. -// Added 19-08-2010 - -//#define UseAirspeed -//#define UseBMP -//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!) - -/**********************************************/ - -/* User definable modules - END */ - -// Frame build condiguration -#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration -//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms - -// Quick and easy hack to change FTDI Serial output to Telemetry port. Just activate #define IsXBEE some lines earlier -#ifndef IsXBEE -#define SerBau 115200 -#define SerPri Serial.print -#define SerPriln Serial.println -#define SerAva Serial.available -#define SerRea Serial.read -#define SerFlu Serial.flush -#define SerBeg Serial.begin -#define SerPor "FTDI" -#else -#define SerBau 115200 -#define SerPri Serial3.print -#define SerPriln Serial3.println -#define SerAva Serial3.available -#define SerRea Serial3.read -#define SerFlu Serial3.flush -#define SerBeg Serial3.begin -#define SerPor "Telemetry" -#endif - -/* ****************************************************************************** */ -/* ****************************** Includes ************************************** */ -/* ****************************************************************************** */ - -#include -#include -APM_ADC_Class APM_ADC; -#include -#include -#include -APM_Compass_Class APM_Compass; -#include -#ifdef UseBMP -#include -APM_BMP085_Class APM_BMP085; -#endif - -#include // General NMEA GPS -//#include // MediaTEK DIY Drones GPS. -//#include // uBlox GPS - -// EEPROM storage for user configurable values -#include -#include "ArduCopter.h" -#include "UserConfig.h" - -/* Software version */ -#define VER 1.34 // Current software version (only numeric values) - - -/* ***************************************************************************** */ -/* ************************ CONFIGURATION PART ********************************* */ -/* ***************************************************************************** */ - -// Normal users does not need to edit anything below these lines. If you have -// need, go and change them in UserConfig.h - -/* ************************************************************ */ -// STABLE MODE -// PI absolute angle control driving a P rate control -// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands -void Attitude_control_v3() -{ - #define MAX_CONTROL_OUTPUT 250 - float stable_roll,stable_pitch,stable_yaw; - - // ROLL CONTROL - if (AP_mode==2) // Normal Mode => Stabilization mode - err_roll = command_rx_roll - ToDeg(roll); - else - err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control - err_roll = constrain(err_roll,-25,25); // to limit max roll command... - - roll_I += err_roll*G_Dt; - roll_I = constrain(roll_I,-20,20); - - // PID absolute angle control - K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain - stable_roll = K_aux*err_roll + KI_QUAD_ROLL*roll_I; - - // PD rate control (we use also the bias corrected gyro rates) - err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected - control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll; - control_roll = constrain(control_roll,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT); - - // PITCH CONTROL - if (AP_mode==2) // Normal mode => Stabilization mode - err_pitch = command_rx_pitch - ToDeg(pitch); - else // GPS Position hold - err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control - err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... - - pitch_I += err_pitch*G_Dt; - pitch_I = constrain(pitch_I,-20,20); - - // PID absolute angle control - K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain - stable_pitch = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I; - - // P rate control (we use also the bias corrected gyro rates) - err_pitch = stable_pitch - ToDeg(Omega[1]); - control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch; - control_pitch = constrain(control_pitch,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT); - - // YAW CONTROL - err_yaw = command_rx_yaw - ToDeg(yaw); - if (err_yaw > 180) // Normalize to -180,180 - err_yaw -= 360; - else if(err_yaw < -180) - err_yaw += 360; - err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command... - - yaw_I += err_yaw*G_Dt; - yaw_I = constrain(yaw_I,-20,20); - - // PID absoulte angle control - stable_yaw = KP_QUAD_YAW*err_yaw + KI_QUAD_YAW*yaw_I; - // PD rate control (we use also the bias corrected gyro rates) - err_yaw = stable_yaw - ToDeg(Omega[2]); - control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw; - control_yaw = constrain(control_yaw,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT); -} - -// ACRO MODE -void Rate_control() -{ - static float previousRollRate, previousPitchRate, previousYawRate; - float currentRollRate, currentPitchRate, currentYawRate; - - // ROLL CONTROL - currentRollRate = read_adc(0); // I need a positive sign here - - err_roll = ((ch_roll - roll_mid) * xmitFactor) - currentRollRate; - - roll_I += err_roll * G_Dt; - roll_I = constrain(roll_I, -20, 20); - - roll_D = currentRollRate - previousRollRate; - previousRollRate = currentRollRate; - - // PID control - control_roll = Kp_RateRoll * err_roll + Kd_RateRoll * roll_D + Ki_RateRoll * roll_I; - - // PITCH CONTROL - currentPitchRate = read_adc(1); - err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate; - - pitch_I += err_pitch*G_Dt; - pitch_I = constrain(pitch_I,-20,20); - - pitch_D = currentPitchRate - previousPitchRate; - previousPitchRate = currentPitchRate; - - // PID control - control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; - - // YAW CONTROL - currentYawRate = read_adc(2); - err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate; - - yaw_I += err_yaw*G_Dt; - yaw_I = constrain(yaw_I, -20, 20); - - yaw_D = currentYawRate - previousYawRate; - previousYawRate = currentYawRate; - - // PID control - K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain - control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; -} - -// RATE CONTROL MODE -// Using Omega vector (bias corrected gyro rate) -void Rate_control_v2() -{ - static float previousRollRate, previousPitchRate, previousYawRate; - float currentRollRate, currentPitchRate, currentYawRate; - - // ROLL CONTROL - currentRollRate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected - - err_roll = ((ch_roll- roll_mid) * xmitFactor) - currentRollRate; - - roll_I += err_roll*G_Dt; - roll_I = constrain(roll_I,-20,20); - - roll_D = (currentRollRate - previousRollRate)/G_Dt; - previousRollRate = currentRollRate; - - // PID control - control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; - - // PITCH CONTROL - currentPitchRate = ToDeg(Omega[1]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected - err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate; - - pitch_I += err_pitch*G_Dt; - pitch_I = constrain(pitch_I,-20,20); - - pitch_D = (currentPitchRate - previousPitchRate)/G_Dt; - previousPitchRate = currentPitchRate; - - // PID control - control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; - - // YAW CONTROL - currentYawRate = ToDeg(Omega[2]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected; - err_yaw = ((ch_yaw - yaw_mid)* xmitFactor) - currentYawRate; - - yaw_I += err_yaw*G_Dt; - yaw_I = constrain(yaw_I,-20,20); - - yaw_D = (currentYawRate - previousYawRate)/G_Dt; - previousYawRate = currentYawRate; - - // PID control - K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain - control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; -} - -// Maximun slope filter for radio inputs... (limit max differences between readings) -int channel_filter(int ch, int ch_old) -{ - int diff_ch_old; - - if (ch_old==0) // ch_old not initialized - return(ch); - diff_ch_old = ch - ch_old; // Difference with old reading - if (diff_ch_old < 0) - { - if (diff_ch_old <- 60) - return(ch_old - 60); // We limit the max difference between readings - } - else - { - if (diff_ch_old > 60) - return(ch_old + 60); - } - return((ch + ch_old) >> 1); // Small filtering - //return(ch); -} - -/* ************************************************************ */ -/* **************** MAIN PROGRAM - SETUP ********************** */ -/* ************************************************************ */ -void setup() -{ - int i, j; - float aux_float[3]; - - pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1) - pinMode(LED_Red,OUTPUT); //Red LED B (PC2) - pinMode(LED_Green,OUTPUT); //Green LED C (PC0) - - pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0) - - pinMode(RELE_pin,OUTPUT); // Rele output - digitalWrite(RELE_pin,LOW); - - APM_RC.Init(); // APM Radio initialization - // RC channels Initialization (Quad motors) - APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped - APM_RC.OutputCh(1,MIN_THROTTLE); - APM_RC.OutputCh(2,MIN_THROTTLE); - APM_RC.OutputCh(3,MIN_THROTTLE); - - // delay(1000); // Wait until frame is not moving after initial power cord has connected - for(i = 0; i <= 50; i++) { - digitalWrite(LED_Green, HIGH); - digitalWrite(LED_Yellow, HIGH); - digitalWrite(LED_Red, HIGH); - delay(20); - digitalWrite(LED_Green, LOW); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, LOW); - delay(20); - } - - APM_ADC.Init(); // APM ADC library initialization - DataFlash.Init(); // DataFlash log initialization - -#ifdef IsGPS - GPS.Init(); // GPS Initialization -#ifdef IsNEWMTEK - delay(250); - // DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware. - // If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode - Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); -#endif -#endif - - readUserConfig(); // Load user configurable items from EEPROM - - // Safety measure for Channel mids - if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500; - if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500; - if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500; - - if (MAGNETOMETER == 1) { - APM_Compass.Init(); // I2C initialization - APM_Compass.SetOrientation(APM_COMPASS_COMPONENTS_UP_PINS_BACK); - APM_Compass.SetOffsets(0,0,0); - APM_Compass.SetDeclination(ToRad(0.0)); - } - - DataFlash.StartWrite(1); // Start a write session on page 1 - - SerBeg(SerBau); // Initialize SerialXX.port, IsXBEE define declares which port -#ifndef CONFIGURATOR - SerPri("ArduCopter Quadcopter v"); - SerPriln(VER); - SerPri("Serial ready on port: "); // Printout greeting to selecter serial port - SerPriln(SerPor); // Printout serial port name -#endif - - // Check if we enable the DataFlash log Read Mode (switch) - // If we press switch 1 at startup we read the Dataflash eeprom - while (digitalRead(SW1_pin)==0) - { - SerPriln("Entering Log Read Mode..."); - Log_Read(1,2000); - delay(30000); - } - - Read_adc_raw(); - delay(10); - - // Offset values for accels and gyros... - AN_OFFSET[3] = acc_offset_x; - AN_OFFSET[4] = acc_offset_y; - AN_OFFSET[5] = acc_offset_z; - aux_float[0] = gyro_offset_roll; - aux_float[1] = gyro_offset_pitch; - aux_float[2] = gyro_offset_yaw; - - j = 0; - // Take the gyro offset values - for(i=0;i<300;i++) - { - Read_adc_raw(); - for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset. - { - aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2; - //SerPri(AN[y]); - //SerPri(","); - } - //SerPriln(); - Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); - delay(10); - - // Runnings lights effect to let user know that we are taking mesurements - if(j == 0) { - digitalWrite(LED_Green, HIGH); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, LOW); - } - else if (j == 1) { - digitalWrite(LED_Green, LOW); - digitalWrite(LED_Yellow, HIGH); - digitalWrite(LED_Red, LOW); - } - else { - digitalWrite(LED_Green, LOW); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, HIGH); - } - if((i % 5) == 0) j++; - if(j >= 3) j = 0; - } - digitalWrite(LED_Green, LOW); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, LOW); - - for(int y=0; y<=2; y++) - AN_OFFSET[y]=aux_float[y]; - - // Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value -#ifndef CONFIGURATOR - for(i=0;i<6;i++) - { - SerPri("AN[]:"); - SerPriln(AN_OFFSET[i]); - } - SerPri("Yaw neutral value:"); - // SerPriln(Neutro_yaw); - SerPri(yaw_mid); -#endif - delay(1000); - - DataFlash.StartWrite(1); // Start a write session on page 1 - timer = millis(); - tlmTimer = millis(); - Read_adc_raw(); // Initialize ADC readings... - delay(20); - -#ifdef IsAM - // Switch Left & Right lights on - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); -#endif - - motorArmed = 0; - digitalWrite(LED_Green,HIGH); // Ready to go... - -} - - -/* ************************************************************ */ -/* ************** MAIN PROGRAM - MAIN LOOP ******************** */ -/* ************************************************************ */ -void loop(){ - - int aux; - int i; - float aux_float; - - //Log variables - int log_roll; - int log_pitch; - int log_yaw; - - if((millis()-timer)>=10) // Main loop 100Hz - { - counter++; - timer_old = timer; - timer=millis(); - G_Dt = (timer-timer_old)*0.001; // Real time of loop run - - // IMU DCM Algorithm - Read_adc_raw(); -#ifdef IsMAG - if (MAGNETOMETER == 1) { - if (counter > 10) // Read compass data at 10Hz... (10 loop runs) - { - counter=0; - APM_Compass.Read(); // Read magnetometer - APM_Compass.Calculate(roll,pitch); // Calculate heading - } - } -#endif - - Matrix_update(); - Normalize(); - Drift_correction(); - Euler_angles(); - - // ***************** - // Output data - log_roll = ToDeg(roll) * 10; - log_pitch = ToDeg(pitch) * 10; - log_yaw = ToDeg(yaw) * 10; - -#ifndef CONFIGURATOR - SerPri(log_roll); - SerPri(","); - SerPri(log_pitch); - SerPri(","); - SerPri(log_yaw); - - //for (int i = 0; i < 6; i++) - //{ - // SerPri(AN[i]); - // SerPri(","); - //} -#endif - - // Write Sensor raw data to DataFlash log - Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp); - // Write attitude to DataFlash log - Log_Write_Attitude(log_roll,log_pitch,log_yaw); - - if (APM_RC.GetState() == 1) // New radio frame? - { - // Commands from radio Rx... - // Stick position defines the desired angle in roll, pitch and yaw - ch_roll = channel_filter(APM_RC.InputCh(0) * ch_roll_slope + ch_roll_offset, ch_roll); - ch_pitch = channel_filter(APM_RC.InputCh(1) * ch_pitch_slope + ch_pitch_offset, ch_pitch); - //ch_throttle = channel_filter(APM_RC.InputCh(2) * ch_throttle_slope + ch_throttle_offset, ch_throttle); - ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle); // Transmiter calibration not used on throttle - ch_yaw = channel_filter(APM_RC.InputCh(3) * ch_yaw_slope + ch_yaw_offset, ch_yaw); - ch_aux = APM_RC.InputCh(4) * ch_aux_slope + ch_aux_offset; - ch_aux2 = APM_RC.InputCh(5) * ch_aux2_slope + ch_aux2_offset; - - command_rx_roll = (ch_roll-roll_mid) / 12.0; - command_rx_pitch = (ch_pitch-pitch_mid) / 12.0; - -#ifdef AUTOMODE - // New Automatic Stable <=> Acro switch. If pitch/roll stick is more than 60% from center, change to Acro - if(command_rx_roll >= 30 || command_rx_roll <= -30 || - command_rx_pitch >= 30 || command_rx_pitch <= -30 ) { - FL_mode = 1; - } else FL_mode = 0; -#endif - - if(ch_aux2 > 1800) FL_mode = 1; // Force to Acro mode from radio - -/* - // Debuging channels and fl_mode - SerPri(command_rx_roll); - comma(); - SerPri(command_rx_pitch); - comma(); - SerPri(FL_mode, DEC); - SerPriln(); -*/ - - - //aux_float = (ch_yaw-Neutro_yaw) / 180.0; - if (abs(ch_yaw-yaw_mid)<12) // Take into account a bit of "dead zone" on yaw - aux_float = 0.0; - else - aux_float = (ch_yaw-yaw_mid) / 180.0; - command_rx_yaw += aux_float; - if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees - command_rx_yaw -= 360.0; - else if (command_rx_yaw < -180) - command_rx_yaw += 360.0; - - // Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains - // I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now] - //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2; - K_aux = K_aux * 0.8 + ((ch_aux2-AUX_MID) / 300.0 + 1.7) * 0.2; // /300 + 1.0 - if (K_aux < 0) K_aux = 0; - //SerPri(","); - //SerPri(K_aux); - - - - - - - // We read the Quad Mode from Channel 5 - if (ch_aux > 1800) // We really need to switch it ON from radio to activate GPS hold - { - AP_mode = 1; // Position hold mode (GPS position control) - digitalWrite(LED_Yellow,HIGH); // Yellow LED On - } - else - { - AP_mode = 2; // Normal mode (Stabilization assist mode) - digitalWrite(LED_Yellow,LOW); // Yellow LED off - } - // Write Radio data to DataFlash log - Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode); - } // END new radio data - - - if (AP_mode==1) // Position Control - { - if (target_position==0) // If this is the first time we switch to Position control, actual position is our target position - { - target_lattitude = GPS.Lattitude; - target_longitude = GPS.Longitude; - -#ifndef CONFIGURATOR - SerPriln(); - SerPri("* Target:"); - SerPri(target_longitude); - SerPri(","); - SerPriln(target_lattitude); -#endif - target_position=1; - //target_sonar_altitude = sonar_value; - //Initial_Throttle = ch3; - // Reset I terms - altitude_I = 0; - gps_roll_I = 0; - gps_pitch_I = 0; - } - } - else - target_position=0; - - //Read GPS - GPS.Read(); - if (GPS.NewData) // New GPS data? - { - GPS_timer_old=GPS_timer; // Update GPS timer - GPS_timer = timer; - GPS_Dt = (GPS_timer-GPS_timer_old)*0.001; // GPS_Dt - GPS.NewData=0; // We Reset the flag... - - //Output GPS data - //SerPri(","); - //SerPri(GPS.Lattitude); - //SerPri(","); - //SerPri(GPS.Longitude); - - // Write GPS data to DataFlash log - Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats); - - //if (GPS.Fix >= 2) - if (GPS.Fix) - digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED - else - digitalWrite(LED_Red,LOW); - - if (AP_mode==1) - { - if ((target_position==1) && (GPS.Fix)) - { - Position_control(target_lattitude,target_longitude); // Call position hold routine - } - else - { - //SerPri("NOFIX"); - command_gps_roll=0; - command_gps_pitch=0; - } - } - } - - - - - - // Control methodology selected using AUX2 -// if (ch_aux2 < 1200) { - if(FL_mode == 0) { // Changed for variable - gled_speed = 1200; - Attitude_control_v3(); - } - else - { - gled_speed = 400; - Rate_control_v2(); - // Reset yaw, so if we change to stable mode we continue with the actual yaw direction - command_rx_yaw = ToDeg(yaw); - } - - // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (ch_throttle < (MIN_THROTTLE + 100)) { - control_yaw = 0; - command_rx_yaw = ToDeg(yaw); - if (ch_yaw > 1850) { - if (Arming_counter > ARM_DELAY){ - if(ch_throttle > 800) { - motorArmed = 1; - minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle - } - } - else - Arming_counter++; - } - else - Arming_counter=0; - // To Disarm motor output : Throttle down and full yaw left for more than 2 seconds - if (ch_yaw < 1150) { - if (Disarming_counter > DISARM_DELAY){ - motorArmed = 0; - minThrottle = MIN_THROTTLE; - } - else - Disarming_counter++; - } - else - Disarming_counter=0; - } - else{ - Arming_counter=0; - Disarming_counter=0; - } - - // Quadcopter mix - if (motorArmed == 1) { -#ifdef IsAM - digitalWrite(FR_LED, HIGH); // AM-Mode -#endif -#ifdef FLIGHT_MODE_+ - rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000); - leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000); - frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000); - backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000); -#endif -#ifdef FLIGHT_MODE_X - rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor - leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor - frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor - backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor -#endif - } - if (motorArmed == 0) { -#ifdef IsAM - digitalWrite(FR_LED, LOW); // AM-Mode -#endif - digitalWrite(LED_Green,HIGH); // Ready LED on - - rightMotor = MIN_THROTTLE; - leftMotor = MIN_THROTTLE; - frontMotor = MIN_THROTTLE; - backMotor = MIN_THROTTLE; - roll_I = 0; // reset I terms of PID controls - pitch_I = 0; - yaw_I = 0; - // Initialize yaw command to actual yaw when throttle is down... - command_rx_yaw = ToDeg(yaw); - } - APM_RC.OutputCh(0, rightMotor); // Right motor - APM_RC.OutputCh(1, leftMotor); // Left motor - APM_RC.OutputCh(2, frontMotor); // Front motor - APM_RC.OutputCh(3, backMotor); // Back motor - - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - -#ifndef CONFIGURATOR - SerPriln(); // Line END -#endif - } -#ifdef CONFIGURATOR - if((millis()-tlmTimer)>=100) { - readSerialCommand(); - sendSerialTelemetry(); - tlmTimer = millis(); - } -#endif - - // AM and Mode status LED lights - if(millis() - gled_timer > gled_speed) { - gled_timer = millis(); - if(gled_status == HIGH) { - digitalWrite(LED_Green, LOW); -#ifdef IsAM - digitalWrite(RE_LED, LOW); -#endif - gled_status = LOW; - } - else { - digitalWrite(LED_Green, HIGH); -#ifdef IsAM - if(motorArmed) digitalWrite(RE_LED, HIGH); -#endif - gled_status = HIGH; - } - } - -} // End of void loop() - -// END of Arducopter.pde - - - diff --git a/legacy_do_not_use/Arducopter/DCM.pde b/legacy_do_not_use/Arducopter/DCM.pde deleted file mode 100644 index 169e4c8117..0000000000 --- a/legacy_do_not_use/Arducopter/DCM.pde +++ /dev/null @@ -1,247 +0,0 @@ -/* - ArduCopter v1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . -*/ - - -/* ******* ADC functions ********************* */ -// Read all the ADC channles -void Read_adc_raw(void) -{ - int temp; - - for (int i=0;i<6;i++) - AN[i] = APM_ADC.Ch(sensors[i]); - - // Correction for non ratiometric sensor (test code) - gyro_temp = APM_ADC.Ch(3); - //AN[0] += 1500-temp; - //AN[1] += 1500-temp; - //AN[2] += 1500-temp; -} - -// Returns an analog value with the offset -int read_adc(int select) -{ - if (SENSOR_SIGN[select]<0) - return (AN_OFFSET[select]-AN[select]); - else - return (AN[select]-AN_OFFSET[select]); -} -/* ******************************************* */ - -/* ******* DCM IMU functions ********************* */ -/**************************************************/ -void Normalize(void) -{ - float error=0; - float temporary[3][3]; - float renorm=0; - - error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 - - Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 - Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 - - Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 - Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 - - Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 - Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 - Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 - Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); -} - -/**************************************************/ -void Drift_correction(void) -{ - //Compensation the Roll, Pitch and Yaw drift. - float errorCourse; - static float Scaled_Omega_P[3]; - static float Scaled_Omega_I[3]; - - //*****Roll and Pitch*************** - - Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference - // errorRollPitch are in Accel ADC units - // Limit max errorRollPitch to limit max Omega_P and Omega_I - errorRollPitch[0] = constrain(errorRollPitch[0],-50,50); - errorRollPitch[1] = constrain(errorRollPitch[1],-50,50); - errorRollPitch[2] = constrain(errorRollPitch[2],-50,50); - Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH); - - Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); - - //*****YAW*************** - // We make the gyro YAW drift correction based on compass magnetic heading - if (MAGNETOMETER == 1) { - errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - - Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I - } - -} -/**************************************************/ -void Accel_adjust(void) -{ - //Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ - //Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY -} -/**************************************************/ - -void Matrix_update(void) -{ - Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll - Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch - Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - - //Accel_Vector[0]=read_adc(3); // acc x - //Accel_Vector[1]=read_adc(4); // acc y - //Accel_Vector[2]=read_adc(5); // acc z - - // Low pass filter on accelerometer data (to filter vibrations) - Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x - Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y - Accel_Vector[2]=Accel_Vector[2]*0.6 + (float)read_adc(5)*0.4; // acc z - - Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator - Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional - - //Accel_adjust();//adjusting centrifugal acceleration. // Not used for quadcopter - - #if OUTPUTMODE==1 // corrected mode - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z - Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y - Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x - Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y - Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x - Update_Matrix[2][2]=0; - #endif - #if OUTPUTMODE==0 // uncorrected data of the gyros (with drift) - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z - Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y - Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; - Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; - Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; - Update_Matrix[2][2]=0; - #endif - - Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c - - for(int x=0; x<3; x++) //Matrix Addition (update) - { - for(int y=0; y<3; y++) - { - DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; - } - } -} - -void Euler_angles(void) -{ - #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) - pitch = -asin((Accel_Vector[0])/(float)GRAVITY); // asin(acc_x) - yaw = 0; - #else // Euler angles from DCM matrix - pitch = asin(-DCM_Matrix[2][0]); - roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - #endif -} - - -// VECTOR FUNCTIONS -//Computes the dot product of two vectors -float Vector_Dot_Product(float vector1[3],float vector2[3]) -{ - float op=0; - - for(int c=0; c<3; c++) - { - op+=vector1[c]*vector2[c]; - } - - return op; -} - -//Computes the cross product of two vectors -void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) -{ - vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); - vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); - vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); -} - -//Multiply the vector by a scalar. -void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn[c]*scale2; - } -} - -void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn1[c]+vectorIn2[c]; - } -} - -/********* MATRIX FUNCTIONS *****************************************/ -//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) -{ - float op[3]; - for(int x=0; x<3; x++) - { - for(int y=0; y<3; y++) - { - for(int w=0; w<3; w++) - { - op[w]=a[x][w]*b[w][y]; - } - mat[x][y]=0; - mat[x][y]=op[0]+op[1]+op[2]; - - float test=mat[x][y]; - } - } -} - - diff --git a/legacy_do_not_use/Arducopter/Functions.pde b/legacy_do_not_use/Arducopter/Functions.pde deleted file mode 100644 index acf737a054..0000000000 --- a/legacy_do_not_use/Arducopter/Functions.pde +++ /dev/null @@ -1,151 +0,0 @@ -/* - ArduCopter 1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . -*/ - -void RadioCalibration() { - long command_timer; - int command; - int counter = 5; - boolean Cmd_ok; - long roll_new = 0; - long pitch_new = 0; - long yaw_new = 0; - - SerFlu(); - SerPriln("Entering Radio Calibration mode"); - SerPriln("Current channel MID values are:"); - SerPri("ROLL: "); - SerPri(roll_mid); - SerPri(" PITCH: "); - SerPri(pitch_mid); - SerPri(" YAW: "); - SerPri(yaw_mid); - SerPriln(); - SerPriln("Recalibrate Channel MID's [Y/N]?: "); - command_timer = millis(); - - // Start counter loop and wait serial input. If not input for 5 seconds, return to normal mode - while(millis() - command_timer < 5000) { - if (SerAva()) { - queryType = SerRea(); - if(queryType == 'y' || queryType == 'Y') { - Cmd_ok = TRUE; - break; - } - else { - Cmd_ok = FALSE; - break; - } - } - } - if(Cmd_ok) { - // We have a go. Let's do new calibration - SerPriln("Starting calibration run in 5 seconds. Place all sticks to their middle including trims"); - for(counter = 5; counter >= 0; counter --) { - command_timer = millis(); - while(millis() - command_timer < 1000) { - } - SerPriln(counter); - } - // Do actual calibration now - SerPriln("Measuring average channel values"); - SerPriln("ROLL, PITCH, YAW"); - - counter = 0; // Reset counter for just in case. - command_timer = millis(); - while(millis() - command_timer < 1000) { - - if (APM_RC.GetState()==1) { // New radio frame? - // Commands from radio Rx... - ch_roll = channel_filter(APM_RC.InputCh(0), ch_roll); - ch_pitch = channel_filter(APM_RC.InputCh(1), ch_pitch); - ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle); - ch_yaw = channel_filter(APM_RC.InputCh(3), ch_yaw); - ch_aux = APM_RC.InputCh(4); - ch_aux2 = APM_RC.InputCh(5); - - SerPri(ch_roll); - comma(); - SerPri(ch_pitch); - comma(); - SerPri(ch_yaw); - SerPriln(); - roll_new += ch_roll; - pitch_new += ch_pitch; - yaw_new += ch_yaw; - counter++; - } - } - SerPri("New samples received: "); - SerPriln(counter); - roll_new = roll_new / counter; - pitch_new = pitch_new / counter; - yaw_new = yaw_new / counter; - SerPri("New values as: "); - SerPri("ROLL: "); - SerPri(roll_new); - SerPri(" PITCH: "); - SerPri(pitch_new); - SerPri(" YAW: "); - SerPri(yaw_new); - SerPriln(); - SerPriln("Accept & Save values [Y/N]?: "); - Cmd_ok = FALSE; - while(millis() - command_timer < 5000) { - if (SerAva()) { - queryType = SerRea(); - if(queryType == 'y' || queryType == 'Y') { - Cmd_ok = TRUE; - roll_mid = roll_new; - pitch_mid = pitch_new; - yaw_mid = yaw_new; - SerPriln("Values accepted, remember to save them to EEPROM with 'W' command"); - break; - } - else { - Cmd_ok = TRUE; - break; - } - } - } - } - if(queryType == 'n' || queryType == 'N') Cmd_ok = TRUE; - if(Cmd_ok) - SerPriln("Returning normal mode..."); - else SerPriln("Command timeout, returning normal mode...."); -} - -void comma() { - SerPri(','); -} - -#if BATTERY_EVENT == 1 -void low_battery_event(void) -{ -// send_message(SEVERITY_HIGH,"Low Battery!"); -// set_mode(RTL); -// throttle_cruise = THROTTLE_CRUISE; -} -#endif - - - - - - diff --git a/legacy_do_not_use/Arducopter/Log.pde b/legacy_do_not_use/Arducopter/Log.pde deleted file mode 100644 index c3db699ea3..0000000000 --- a/legacy_do_not_use/Arducopter/Log.pde +++ /dev/null @@ -1,267 +0,0 @@ -// Test code to Write and Read packets from DataFlash log memory -// Packets : Attitude, Raw sensor data, Radio and GPS - -#define HEAD_BYTE1 0xA3 -#define HEAD_BYTE2 0x95 -#define END_BYTE 0xBA - -#define LOG_ATTITUDE_MSG 0x01 -#define LOG_GPS_MSG 0x02 -#define LOG_RADIO_MSG 0x03 -#define LOG_SENSOR_MSG 0x04 -#define LOG_PID_MSG 0x05 - -// Write a Sensor Raw data packet -void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_SENSOR_MSG); - DataFlash.WriteInt(s1); - DataFlash.WriteInt(s2); - DataFlash.WriteInt(s3); - DataFlash.WriteInt(s4); - DataFlash.WriteInt(s5); - DataFlash.WriteInt(s6); - DataFlash.WriteInt(s7); - DataFlash.WriteByte(END_BYTE); -} - -// Write an attitude packet. Total length : 10 bytes -void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - DataFlash.WriteInt(log_roll); - DataFlash.WriteInt(log_pitch); - DataFlash.WriteInt(log_yaw); - DataFlash.WriteByte(END_BYTE); -} - -// Write a PID control info -void Log_Write_PID(byte num_PID, int P, int I,int D, int output) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PID_MSG); - DataFlash.WriteByte(num_PID); - DataFlash.WriteInt(P); - DataFlash.WriteInt(I); - DataFlash.WriteInt(D); - DataFlash.WriteInt(output); - DataFlash.WriteByte(END_BYTE); -} - -// Write an GPS packet. Total length : 30 bytes -void Log_Write_GPS(long log_Time, long log_Lattitude, long log_Longitude, long log_Altitude, - long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_GPS_MSG); - DataFlash.WriteLong(log_Time); - DataFlash.WriteByte(log_Fix); - DataFlash.WriteByte(log_NumSats); - DataFlash.WriteLong(log_Lattitude); - DataFlash.WriteLong(log_Longitude); - DataFlash.WriteLong(log_Altitude); - DataFlash.WriteLong(log_Ground_Speed); - DataFlash.WriteLong(log_Ground_Course); - DataFlash.WriteByte(END_BYTE); -} - -// Write a Radio packet -void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_RADIO_MSG); - DataFlash.WriteInt(ch1); - DataFlash.WriteInt(ch2); - DataFlash.WriteInt(ch3); - DataFlash.WriteInt(ch4); - DataFlash.WriteInt(ch5); - DataFlash.WriteInt(ch6); - DataFlash.WriteByte(END_BYTE); -} - -// **** READ ROUTINES **** - -// Read a Sensor raw data packet -void Log_Read_Sensor() -{ - SerPri("SENSOR:"); - SerPri(DataFlash.ReadInt()); // GX - SerPri(","); - SerPri(DataFlash.ReadInt()); // GY - SerPri(","); - SerPri(DataFlash.ReadInt()); // GZ - SerPri(","); - SerPri(DataFlash.ReadInt()); // ACCX - SerPri(","); - SerPri(DataFlash.ReadInt()); // ACCY - SerPri(","); - SerPri(DataFlash.ReadInt()); // ACCZ - SerPri(","); - SerPri(DataFlash.ReadInt()); // AUX - SerPriln(); -} - -// Read an attitude packet -void Log_Read_Attitude() -{ - int log_roll; - int log_pitch; - int log_yaw; - - log_roll = DataFlash.ReadInt(); - log_pitch = DataFlash.ReadInt(); - log_yaw = DataFlash.ReadInt(); - SerPri("ATT:"); - SerPri(log_roll); - SerPri(","); - SerPri(log_pitch); - SerPri(","); - SerPri(log_yaw); - SerPriln(); -} - -// Read a Sensor raw data packet -void Log_Read_PID() -{ - SerPri("PID:"); - SerPri((int)DataFlash.ReadByte()); // NUM_PID - SerPri(","); - SerPri(DataFlash.ReadInt()); // P - SerPri(","); - SerPri(DataFlash.ReadInt()); // I - SerPri(","); - SerPri(DataFlash.ReadInt()); // D - SerPri(","); - SerPri(DataFlash.ReadInt()); // output - SerPriln(); -} - -// Read a GPS packet -void Log_Read_GPS() -{ - long log_Time; - byte log_Fix; - byte log_NumSats; - long log_Lattitude; - long log_Longitude; - long log_Altitude; - long log_Ground_Speed; - long log_Ground_Course; - - log_Time = DataFlash.ReadLong(); - log_Fix = DataFlash.ReadByte(); - log_NumSats = DataFlash.ReadByte(); - log_Lattitude = DataFlash.ReadLong(); - log_Longitude = DataFlash.ReadLong(); - log_Altitude = DataFlash.ReadLong(); - log_Ground_Speed = DataFlash.ReadLong(); - log_Ground_Course = DataFlash.ReadLong(); - - SerPri("GPS:"); - SerPri(log_Time); - SerPri(","); - SerPri((int)log_Fix); - SerPri(","); - SerPri((int)log_NumSats); - SerPri(","); - SerPri(log_Lattitude); - SerPri(","); - SerPri(log_Longitude); - SerPri(","); - SerPri(log_Altitude); - SerPri(","); - SerPri(log_Ground_Speed); - SerPri(","); - SerPri(log_Ground_Course); - SerPriln(); - -} - -// Read an Radio packet -void Log_Read_Radio() -{ - SerPri("RADIO:"); - SerPri(DataFlash.ReadInt()); - SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPriln(); -} - -// Read the DataFlash log memory : Packet Parser -void Log_Read(int start_page, int end_page) -{ - byte data; - byte log_step=0; - long packet_count=0; - - DataFlash.StartRead(start_page); - while (DataFlash.GetPage() < end_page) - { - data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets - { - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - break; - case 2: - switch (data) - { - case LOG_ATTITUDE_MSG: - Log_Read_Attitude(); - log_step++; - break; - case LOG_GPS_MSG: - Log_Read_GPS(); - log_step++; - break; - case LOG_RADIO_MSG: - Log_Read_Radio(); - log_step++; - break; - case LOG_SENSOR_MSG: - Log_Read_Sensor(); - log_step++; - break; - case LOG_PID_MSG: - Log_Read_PID(); - log_step++; - break; - default: - SerPri("Error Reading Packet: "); - SerPri(packet_count); - log_step=0; // Restart, we have a problem... - } - break; - case 3: - if(data==END_BYTE) - packet_count++; - else - SerPriln("Error Reading END_BYTE"); - log_step=0; // Restart sequence: new packet... - } - } - SerPri("Number of packets read: "); - SerPriln(packet_count); -} - - diff --git a/legacy_do_not_use/Arducopter/Navigation.pde b/legacy_do_not_use/Arducopter/Navigation.pde deleted file mode 100644 index 489fe26e78..0000000000 --- a/legacy_do_not_use/Arducopter/Navigation.pde +++ /dev/null @@ -1,73 +0,0 @@ -/* - ArduCopter v1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . -*/ - -/* ************************************************************ */ -/* GPS based Position control */ -void Position_control(long lat_dest, long lon_dest) -{ - long Lon_diff; - long Lat_diff; - - Lon_diff = lon_dest - GPS.Longitude; - Lat_diff = lat_dest - GPS.Lattitude; - - // ROLL - //Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] - gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0]; - - gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt; - gps_err_roll_old = gps_err_roll; - - gps_roll_I += gps_err_roll * GPS_Dt; - gps_roll_I = constrain(gps_roll_I, -800, 800); - - command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I; - command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command - - //Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10); - - // PITCH - gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0]; - - gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt; - gps_err_pitch_old = gps_err_pitch; - - gps_pitch_I += gps_err_pitch * GPS_Dt; - gps_pitch_I = constrain(gps_pitch_I, -800, 800); - - command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I; - command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command - - //Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10); -} - -/* ************************************************************ */ -/* Altitude control... (based on sonar) // NOT TESTED!!*/ -void Altitude_control(int target_sonar_altitude) -{ - err_altitude_old = err_altitude; - err_altitude = target_sonar_altitude - Sonar_value; - altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt; - altitude_I += (float)err_altitude * G_Dt; - altitude_I = constrain(altitude_I, -100, 100); - command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I; -} - - diff --git a/legacy_do_not_use/Arducopter/Sensors.pde b/legacy_do_not_use/Arducopter/Sensors.pde deleted file mode 100644 index 20a5dd186b..0000000000 --- a/legacy_do_not_use/Arducopter/Sensors.pde +++ /dev/null @@ -1,72 +0,0 @@ -/* - ArduCopter v1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . - */ - -void ReadSCP1000(void) { -} - -#ifdef UseBMP -void read_airpressure(void){ - double x; - - APM_BMP085.Read(); //Get new data from absolute pressure sensor - abs_press = APM_BMP085.Press; - abs_press_filt = (abs_press); // + 2l * abs_press_filt) / 3l; //Light filtering - //temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation - - double p = (double)abs_press_gnd / (double)abs_press_filt; - double temp = (float)ground_temperature / 10.f + 273.15f; - x = log(p) * temp * 29271.267f; - //x = log(p) * temp * 29.271267 * 1000; - press_alt = (int)(x / 10) + ground_alt; // Pressure altitude in centimeters - // Need to add comments for theory..... -} -#endif - -#ifdef UseAirspeed -void read_airspeed(void) { -#if GCS_PROTOCOL != 3 // Xplane will supply the airspeed - airpressure_raw = ((float)analogRead(AIRSPEED_PIN) * .25) + (airpressure_raw * .75); - airpressure = (int)airpressure_raw - airpressure_offset; - airspeed = sqrt((float)airpressure / AIRSPEED_RATIO); -#endif - airspeed_error = airspeed_cruise - airspeed; -} -#endif - -#if BATTERY_EVENT == 1 -void read_battery(void) -{ - battery_voltage = BATTERY_VOLTAGE(analogRead(BATTERY_PIN)) * .1 + battery_voltage * .9; - if(battery_voltage < LOW_VOLTAGE) - low_battery_event(); -} -#endif - -#ifdef UseAirspeed -void zero_airspeed(void) -{ - airpressure_raw = analogRead(AIRSPEED_PIN); - for(int c=0; c < 80; c++){ - airpressure_raw = (airpressure_raw * .90) + ((float)analogRead(AIRSPEED_PIN) * .10); - } - airpressure_offset = airpressure_raw; -} -#endif - diff --git a/legacy_do_not_use/Arducopter/SerialCom.pde b/legacy_do_not_use/Arducopter/SerialCom.pde deleted file mode 100644 index 0862766327..0000000000 --- a/legacy_do_not_use/Arducopter/SerialCom.pde +++ /dev/null @@ -1,395 +0,0 @@ -/* - ArduCopter v1.3 - August 2010 - www.ArduCopter.com - Copyright (c) 2010. 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 . -*/ - -void readSerialCommand() { - // Check for serial message - if (SerAva()) { - queryType = SerRea(); - switch (queryType) { - case 'A': // Stable PID - KP_QUAD_ROLL = readFloatSerial(); - KI_QUAD_ROLL = readFloatSerial(); - STABLE_MODE_KP_RATE_ROLL = readFloatSerial(); - KP_QUAD_PITCH = readFloatSerial(); - KI_QUAD_PITCH = readFloatSerial(); - STABLE_MODE_KP_RATE_PITCH = readFloatSerial(); - KP_QUAD_YAW = readFloatSerial(); - KI_QUAD_YAW = readFloatSerial(); - STABLE_MODE_KP_RATE_YAW = readFloatSerial(); - STABLE_MODE_KP_RATE = readFloatSerial(); // NOT USED NOW - MAGNETOMETER = readFloatSerial(); - break; - case 'C': // Receive GPS PID - KP_GPS_ROLL = readFloatSerial(); - KI_GPS_ROLL = readFloatSerial(); - KD_GPS_ROLL = readFloatSerial(); - KP_GPS_PITCH = readFloatSerial(); - KI_GPS_PITCH = readFloatSerial(); - KD_GPS_PITCH = readFloatSerial(); - GPS_MAX_ANGLE = readFloatSerial(); - GEOG_CORRECTION_FACTOR = readFloatSerial(); - break; - case 'E': // Receive altitude PID - KP_ALTITUDE = readFloatSerial(); - KD_ALTITUDE = readFloatSerial(); - KI_ALTITUDE = readFloatSerial(); - break; - case 'G': // Receive drift correction PID - Kp_ROLLPITCH = readFloatSerial(); - Ki_ROLLPITCH = readFloatSerial(); - Kp_YAW = readFloatSerial(); - Ki_YAW = readFloatSerial(); - break; - case 'I': // Receive sensor offset - gyro_offset_roll = readFloatSerial(); - gyro_offset_pitch = readFloatSerial(); - gyro_offset_yaw = readFloatSerial(); - acc_offset_x = readFloatSerial(); - acc_offset_y = readFloatSerial(); - acc_offset_z = readFloatSerial(); - break; - case 'K': // Spare - break; - case 'M': // Receive debug motor commands - frontMotor = readFloatSerial(); - backMotor = readFloatSerial(); - rightMotor = readFloatSerial(); - leftMotor = readFloatSerial(); - motorArmed = readFloatSerial(); - break; - case 'O': // Rate Control PID - Kp_RateRoll = readFloatSerial(); - Ki_RateRoll = readFloatSerial(); - Kd_RateRoll = readFloatSerial(); - Kp_RatePitch = readFloatSerial(); - Ki_RatePitch = readFloatSerial(); - Kd_RatePitch = readFloatSerial(); - Kp_RateYaw = readFloatSerial(); - Ki_RateYaw = readFloatSerial(); - Kd_RateYaw = readFloatSerial(); - xmitFactor = readFloatSerial(); - break; - case 'W': // Write all user configurable values to EEPROM - writeUserConfig(); - break; - case 'Y': // Initialize EEPROM with default values - defaultUserConfig(); - break; - case '1': // Receive transmitter calibration values - ch_roll_slope = readFloatSerial(); - ch_roll_offset = readFloatSerial(); - ch_pitch_slope = readFloatSerial(); - ch_pitch_offset = readFloatSerial(); - ch_yaw_slope = readFloatSerial(); - ch_yaw_offset = readFloatSerial(); - ch_throttle_slope = readFloatSerial(); - ch_throttle_offset = readFloatSerial(); - ch_aux_slope = readFloatSerial(); - ch_aux_offset = readFloatSerial(); - ch_aux2_slope = readFloatSerial(); - ch_aux2_offset = readFloatSerial(); - break; - } - } -} - -void sendSerialTelemetry() { - float aux_float[3]; // used for sensor calibration - switch (queryType) { - case '=': // Reserved debug command to view any variable from Serial Monitor -/* SerPri("throttle ="); - SerPriln(ch_throttle); - SerPri("control roll ="); - SerPriln(control_roll-CHANN_CENTER); - SerPri("control pitch ="); - SerPriln(control_pitch-CHANN_CENTER); - SerPri("control yaw ="); - SerPriln(control_yaw-CHANN_CENTER); - SerPri("front left yaw ="); - SerPriln(frontMotor); - SerPri("front right yaw ="); - SerPriln(rightMotor); - SerPri("rear left yaw ="); - SerPriln(leftMotor); - SerPri("rear right motor ="); - SerPriln(backMotor); - SerPriln(); - - SerPri("current roll rate ="); - SerPriln(read_adc(0)); - SerPri("current pitch rate ="); - SerPriln(read_adc(1)); - SerPri("current yaw rate ="); - SerPriln(read_adc(2)); - SerPri("command rx yaw ="); - SerPriln(command_rx_yaw); - SerPriln(); - queryType = 'X';*/ - SerPri(APM_RC.InputCh(0)); - comma(); - SerPri(ch_roll_slope); - comma(); - SerPri(ch_roll_offset); - comma(); - SerPriln(ch_roll); - break; - case 'B': // Send roll, pitch and yaw PID values - SerPri(KP_QUAD_ROLL, 3); - comma(); - SerPri(KI_QUAD_ROLL, 3); - comma(); - SerPri(STABLE_MODE_KP_RATE_ROLL, 3); - comma(); - SerPri(KP_QUAD_PITCH, 3); - comma(); - SerPri(KI_QUAD_PITCH, 3); - comma(); - SerPri(STABLE_MODE_KP_RATE_PITCH, 3); - comma(); - SerPri(KP_QUAD_YAW, 3); - comma(); - SerPri(KI_QUAD_YAW, 3); - comma(); - SerPri(STABLE_MODE_KP_RATE_YAW, 3); - comma(); - SerPri(STABLE_MODE_KP_RATE, 3); // NOT USED NOW - comma(); - SerPriln(MAGNETOMETER, 3); - queryType = 'X'; - break; - case 'D': // Send GPS PID - SerPri(KP_GPS_ROLL, 3); - comma(); - SerPri(KI_GPS_ROLL, 3); - comma(); - SerPri(KD_GPS_ROLL, 3); - comma(); - SerPri(KP_GPS_PITCH, 3); - comma(); - SerPri(KI_GPS_PITCH, 3); - comma(); - SerPri(KD_GPS_PITCH, 3); - comma(); - SerPri(GPS_MAX_ANGLE, 3); - comma(); - SerPriln(GEOG_CORRECTION_FACTOR, 3); - queryType = 'X'; - break; - case 'F': // Send altitude PID - SerPri(KP_ALTITUDE, 3); - comma(); - SerPri(KI_ALTITUDE, 3); - comma(); - SerPriln(KD_ALTITUDE, 3); - queryType = 'X'; - break; - case 'H': // Send drift correction PID - SerPri(Kp_ROLLPITCH, 4); - comma(); - SerPri(Ki_ROLLPITCH, 7); - comma(); - SerPri(Kp_YAW, 4); - comma(); - SerPriln(Ki_YAW, 6); - queryType = 'X'; - break; - case 'J': // Send sensor offset - SerPri(gyro_offset_roll); - comma(); - SerPri(gyro_offset_pitch); - comma(); - SerPri(gyro_offset_yaw); - comma(); - SerPri(acc_offset_x); - comma(); - SerPri(acc_offset_y); - comma(); - SerPriln(acc_offset_z); - AN_OFFSET[3] = acc_offset_x; - AN_OFFSET[4] = acc_offset_y; - AN_OFFSET[5] = acc_offset_z; - queryType = 'X'; - break; - case 'L': // Spare - RadioCalibration(); - queryType = 'X'; - break; - case 'N': // Send magnetometer config - queryType = 'X'; - break; - case 'P': // Send rate control PID - SerPri(Kp_RateRoll, 3); - comma(); - SerPri(Ki_RateRoll, 3); - comma(); - SerPri(Kd_RateRoll, 3); - comma(); - SerPri(Kp_RatePitch, 3); - comma(); - SerPri(Ki_RatePitch, 3); - comma(); - SerPri(Kd_RatePitch, 3); - comma(); - SerPri(Kp_RateYaw, 3); - comma(); - SerPri(Ki_RateYaw, 3); - comma(); - SerPri(Kd_RateYaw, 3); - comma(); - SerPriln(xmitFactor, 3); - queryType = 'X'; - break; - case 'Q': // Send sensor data - SerPri(read_adc(0)); - comma(); - SerPri(read_adc(1)); - comma(); - SerPri(read_adc(2)); - comma(); - SerPri(read_adc(4)); - comma(); - SerPri(read_adc(3)); - comma(); - SerPri(read_adc(5)); - comma(); - SerPri(err_roll); - comma(); - SerPri(err_pitch); - comma(); - SerPri(degrees(roll)); - comma(); - SerPri(degrees(pitch)); - comma(); - SerPriln(degrees(yaw)); - break; - case 'R': // Send raw sensor data - break; - case 'S': // Send all flight data - SerPri(timer-timer_old); - comma(); - SerPri(read_adc(0)); - comma(); - SerPri(read_adc(1)); - comma(); - SerPri(read_adc(2)); - comma(); - SerPri(ch_throttle); - comma(); - SerPri(control_roll); - comma(); - SerPri(control_pitch); - comma(); - SerPri(control_yaw); - comma(); - SerPri(frontMotor); // Front Motor - comma(); - SerPri(backMotor); // Back Motor - comma(); - SerPri(rightMotor); // Right Motor - comma(); - SerPri(leftMotor); // Left Motor - comma(); - SerPri(read_adc(4)); - comma(); - SerPri(read_adc(3)); - comma(); - SerPriln(read_adc(5)); - break; - case 'T': // Spare - break; - case 'U': // Send receiver values - SerPri(ch_roll); // Aileron - comma(); - SerPri(ch_pitch); // Elevator - comma(); - SerPri(ch_yaw); // Yaw - comma(); - SerPri(ch_throttle); // Throttle - comma(); - SerPri(ch_aux); // AUX1 (Mode) - comma(); - SerPri(ch_aux2); // AUX2 - comma(); - SerPri(roll_mid); // Roll MID value - comma(); - SerPri(pitch_mid); // Pitch MID value - comma(); - SerPriln(yaw_mid); // Yaw MID Value - break; - case 'V': // Spare - break; - case 'X': // Stop sending messages - break; - case '!': // Send flight software version - SerPriln(VER); - queryType = 'X'; - break; - case '2': // Send transmitter calibration values - SerPri(ch_roll_slope); - comma(); - SerPri(ch_roll_offset); - comma(); - SerPri(ch_pitch_slope); - comma(); - SerPri(ch_pitch_offset); - comma(); - SerPri(ch_yaw_slope); - comma(); - SerPri(ch_yaw_offset); - comma(); - SerPri(ch_throttle_slope); - comma(); - SerPri(ch_throttle_offset); - comma(); - SerPri(ch_aux_slope); - comma(); - SerPri(ch_aux_offset); - comma(); - SerPri(ch_aux2_slope); - comma(); - SerPriln(ch_aux2_offset); - queryType = 'X'; - break; - case '.': // Modify GPS settings - Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); - break; - } -} - -// Used to read floating point values from the serial port -float readFloatSerial() { - byte index = 0; - byte timeout = 0; - char data[128] = ""; - - do { - if (SerAva() == 0) { - delay(10); - timeout++; - } - else { - data[index] = SerRea(); - timeout = 0; - index++; - } - } - while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); - return atof(data); -} diff --git a/legacy_do_not_use/Arducopter/UserConfig.h b/legacy_do_not_use/Arducopter/UserConfig.h deleted file mode 100644 index d80fde8ef5..0000000000 --- a/legacy_do_not_use/Arducopter/UserConfig.h +++ /dev/null @@ -1,399 +0,0 @@ -/* - www.ArduCopter.com - www.DIYDrones.com - Copyright (c) 2010. All rights reserved. - An Open Source Arduino based multicopter. - - File : UserConfig.h - Version : v1.0, Aug 27, 2010 - Author(s): ArduCopter Team - Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, - Jani Hirvinen, Ken McEwans, Roberto Navoni, - Sandro Benigno, Chris Anderson - - 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 . - -* ************************************************************** * -ChangeLog: - -- 27-08-2010, New header layout - -* ************************************************************** * -TODO: - -- List of thigs -- that still need to be done - -* ************************************************************** */ - - - -/*************************************************************/ -// Safety & Security - -// Arm & Disarm delays -#define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors -#define DISARM_DELAY 100 // milliseconds of how long you need to keep rudder to max left for disarming motors - - -/*************************************************************/ -// AM Mode & Flight information - -/* AM PIN Definitions */ -/* Will be moved in future to AN extension ports */ -/* due need to have PWM pins free for sonars and servos */ - -#define FR_LED 3 // Mega PE4 pin, OUT7 -#define RE_LED 2 // Mega PE5 pin, OUT6 -#define RI_LED 7 // Mega PH4 pin, OUT5 -#define LE_LED 8 // Mega PH5 pin, OUT4 - -/* AM PIN Definitions - END */ - - -/*************************************************************/ -// Radio related definitions - -// If you don't know these values, you can activate RADIO_TEST_MODE below -// and check your mid values - -//#define RADIO_TEST_MODE - -#define ROLL_MID 1500 // Radio Roll channel mid value -#define PITCH_MID 1500 // Radio Pitch channel mid value -#define YAW_MID 1500 // Radio Yaw channel mid value -#define THROTTLE_MID 1505 // Radio Throttle channel mid value -#define AUX_MID 1500 - -#define CHANN_CENTER 1500 // Channel center, legacy -#define MIN_THROTTLE 1040 // Throttle pulse width at minimun... - -// Following variables stored in EEPROM -float KP_QUAD_ROLL; -float KI_QUAD_ROLL; -float STABLE_MODE_KP_RATE_ROLL; -float KP_QUAD_PITCH; -float KI_QUAD_PITCH; -float STABLE_MODE_KP_RATE_PITCH; -float KP_QUAD_YAW; -float KI_QUAD_YAW; -float STABLE_MODE_KP_RATE_YAW; -float STABLE_MODE_KP_RATE; // NOT USED NOW -float KP_GPS_ROLL; -float KI_GPS_ROLL; -float KD_GPS_ROLL; -float KP_GPS_PITCH; -float KI_GPS_PITCH; -float KD_GPS_PITCH; -float GPS_MAX_ANGLE; -float KP_ALTITUDE; -float KI_ALTITUDE; -float KD_ALTITUDE; -int acc_offset_x; -int acc_offset_y; -int acc_offset_z; -int gyro_offset_roll; -int gyro_offset_pitch; -int gyro_offset_yaw; -float Kp_ROLLPITCH; -float Ki_ROLLPITCH; -float Kp_YAW; -float Ki_YAW; -float GEOG_CORRECTION_FACTOR; -int MAGNETOMETER; -float Kp_RateRoll; -float Ki_RateRoll; -float Kd_RateRoll; -float Kp_RatePitch; -float Ki_RatePitch; -float Kd_RatePitch; -float Kp_RateYaw; -float Ki_RateYaw; -float Kd_RateYaw; -float xmitFactor; -float ch_roll_slope = 1; -float ch_pitch_slope = 1; -float ch_throttle_slope = 1; -float ch_yaw_slope = 1; -float ch_aux_slope = 1; -float ch_aux2_slope = 1; -float ch_roll_offset = 0; -float ch_pitch_offset = 0; -float ch_throttle_offset = 0; -float ch_yaw_offset = 0; -float ch_aux_offset = 0; -float ch_aux2_offset = 0; - -// This function call contains the default values that are set to the ArduCopter -// when a "Default EEPROM Value" command is sent through serial interface -void defaultUserConfig() { - KP_QUAD_ROLL = 4.0; - KI_QUAD_ROLL = 0.15; - STABLE_MODE_KP_RATE_ROLL = 1.2; - KP_QUAD_PITCH = 4.0; - KI_QUAD_PITCH = 0.15; - STABLE_MODE_KP_RATE_PITCH = 1.2; - KP_QUAD_YAW = 3.0; - KI_QUAD_YAW = 0.15; - STABLE_MODE_KP_RATE_YAW = 2.4; - STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW - KP_GPS_ROLL = 0.015; - KI_GPS_ROLL = 0.005; - KD_GPS_ROLL = 0.01; - KP_GPS_PITCH = 0.015; - KI_GPS_PITCH = 0.005; - KD_GPS_PITCH = 0.01; - GPS_MAX_ANGLE = 22; - KP_ALTITUDE = 0.8; - KI_ALTITUDE = 0.2; - KD_ALTITUDE = 0.2; - acc_offset_x = 2073; - acc_offset_y = 2056; - acc_offset_z = 2010; - gyro_offset_roll = 1659; - gyro_offset_pitch = 1618; - gyro_offset_yaw = 1673; - Kp_ROLLPITCH = 0.0014; - Ki_ROLLPITCH = 0.00000015; - Kp_YAW = 1.2; - Ki_YAW = 0.00005; - GEOG_CORRECTION_FACTOR = 0.87; - MAGNETOMETER = 0; - Kp_RateRoll = 1.95; - Ki_RateRoll = 0.0; - Kd_RateRoll = 0.0; - Kp_RatePitch = 1.95; - Ki_RatePitch = 0.0; - Kd_RatePitch = 0.0; - Kp_RateYaw = 3.2; - Ki_RateYaw = 0.0; - Kd_RateYaw = 0.0; - xmitFactor = 0.32; - roll_mid = 1500; - pitch_mid = 1500; - yaw_mid = 1500; - ch_roll_slope = 1; - ch_pitch_slope = 1; - ch_throttle_slope = 1; - ch_yaw_slope = 1; - ch_aux_slope = 1; - ch_aux2_slope = 1; - ch_roll_offset = 0; - ch_pitch_offset = 0; - ch_throttle_offset = 0; - ch_yaw_offset = 0; - ch_aux_offset = 0; - ch_aux2_offset = 0; -} - -// EEPROM storage addresses -#define KP_QUAD_ROLL_ADR 0 -#define KI_QUAD_ROLL_ADR 8 -#define STABLE_MODE_KP_RATE_ROLL_ADR 4 -#define KP_QUAD_PITCH_ADR 12 -#define KI_QUAD_PITCH_ADR 20 -#define STABLE_MODE_KP_RATE_PITCH_ADR 16 -#define KP_QUAD_YAW_ADR 24 -#define KI_QUAD_YAW_ADR 32 -#define STABLE_MODE_KP_RATE_YAW_ADR 28 -#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW -#define KP_GPS_ROLL_ADR 40 -#define KI_GPS_ROLL_ADR 48 -#define KD_GPS_ROLL_ADR 44 -#define KP_GPS_PITCH_ADR 52 -#define KI_GPS_PITCH_ADR 60 -#define KD_GPS_PITCH_ADR 56 -#define GPS_MAX_ANGLE_ADR 64 -#define KP_ALTITUDE_ADR 68 -#define KI_ALTITUDE_ADR 76 -#define KD_ALTITUDE_ADR 72 -#define acc_offset_x_ADR 80 -#define acc_offset_y_ADR 84 -#define acc_offset_z_ADR 88 -#define gyro_offset_roll_ADR 92 -#define gyro_offset_pitch_ADR 96 -#define gyro_offset_yaw_ADR 100 -#define Kp_ROLLPITCH_ADR 104 -#define Ki_ROLLPITCH_ADR 108 -#define Kp_YAW_ADR 112 -#define Ki_YAW_ADR 116 -#define GEOG_CORRECTION_FACTOR_ADR 120 -#define MAGNETOMETER_ADR 124 -#define XMITFACTOR_ADR 128 -#define KP_RATEROLL_ADR 132 -#define KI_RATEROLL_ADR 136 -#define KD_RATEROLL_ADR 140 -#define KP_RATEPITCH_ADR 144 -#define KI_RATEPITCH_ADR 148 -#define KD_RATEPITCH_ADR 152 -#define KP_RATEYAW_ADR 156 -#define KI_RATEYAW_ADR 160 -#define KD_RATEYAW_ADR 164 -#define CHROLL_MID 168 -#define CHPITCH_MID 172 -#define CHYAW_MID 176 -#define ch_roll_slope_ADR 180 -#define ch_pitch_slope_ADR 184 -#define ch_throttle_slope_ADR 188 -#define ch_yaw_slope_ADR 192 -#define ch_aux_slope_ADR 196 -#define ch_aux2_slope_ADR 200 -#define ch_roll_offset_ADR 204 -#define ch_pitch_offset_ADR 208 -#define ch_throttle_offset_ADR 212 -#define ch_yaw_offset_ADR 216 -#define ch_aux_offset_ADR 220 -#define ch_aux2_offset_ADR 224 - -// Utilities for writing and reading from the EEPROM -float readEEPROM(int address) { - union floatStore { - byte floatByte[4]; - float floatVal; - } floatOut; - - for (int i = 0; i < 4; i++) - floatOut.floatByte[i] = EEPROM.read(address + i); - return floatOut.floatVal; -} - -void writeEEPROM(float value, int address) { - union floatStore { - byte floatByte[4]; - float floatVal; - } floatIn; - - floatIn.floatVal = value; - for (int i = 0; i < 4; i++) - EEPROM.write(address + i, floatIn.floatByte[i]); -} - -void readUserConfig() { - KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); - KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); - STABLE_MODE_KP_RATE_ROLL = readEEPROM(STABLE_MODE_KP_RATE_ROLL_ADR); - KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); - KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); - STABLE_MODE_KP_RATE_PITCH = readEEPROM(STABLE_MODE_KP_RATE_PITCH_ADR); - KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR); - KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR); - STABLE_MODE_KP_RATE_YAW = readEEPROM(STABLE_MODE_KP_RATE_YAW_ADR); - STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); // NOT USED NOW - KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); - KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); - KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); - KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR); - KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR); - KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR); - GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR); - KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR); - KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR); - KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR); - acc_offset_x = readEEPROM(acc_offset_x_ADR); - acc_offset_y = readEEPROM(acc_offset_y_ADR); - acc_offset_z = readEEPROM(acc_offset_z_ADR); - gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR); - gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR); - gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR); - Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR); - Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR); - Kp_YAW = readEEPROM(Kp_YAW_ADR); - Ki_YAW = readEEPROM(Ki_YAW_ADR); - GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR); - MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR); - Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR); - Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR); - Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR); - Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR); - Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR); - Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR); - Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR); - Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR); - Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR); - xmitFactor = readEEPROM(XMITFACTOR_ADR); - roll_mid = readEEPROM(CHROLL_MID); - pitch_mid = readEEPROM(CHPITCH_MID); - yaw_mid = readEEPROM(CHYAW_MID); - ch_roll_slope = readEEPROM(ch_roll_slope_ADR); - ch_pitch_slope = readEEPROM(ch_pitch_slope_ADR); - ch_throttle_slope = readEEPROM(ch_throttle_slope_ADR); - ch_yaw_slope = readEEPROM(ch_yaw_slope_ADR); - ch_aux_slope = readEEPROM(ch_aux_slope_ADR); - ch_aux2_slope = readEEPROM(ch_aux2_slope_ADR); - ch_roll_offset = readEEPROM(ch_roll_offset_ADR); - ch_pitch_offset = readEEPROM(ch_pitch_offset_ADR); - ch_throttle_offset = readEEPROM(ch_throttle_offset_ADR); - ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR); - ch_aux_offset = readEEPROM(ch_aux_offset_ADR); - ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR); -} - -void writeUserConfig() { - writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR); - writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR); - writeEEPROM(STABLE_MODE_KP_RATE_ROLL, STABLE_MODE_KP_RATE_ROLL_ADR); - writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR); - writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR); - writeEEPROM(STABLE_MODE_KP_RATE_PITCH, STABLE_MODE_KP_RATE_PITCH_ADR); - writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR); - writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR); - writeEEPROM(STABLE_MODE_KP_RATE_YAW, STABLE_MODE_KP_RATE_YAW_ADR); - writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR); // NOT USED NOW - writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR); - writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR); - writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR); - writeEEPROM(KP_GPS_PITCH, KP_GPS_PITCH_ADR); - writeEEPROM(KD_GPS_PITCH, KD_GPS_PITCH_ADR); - writeEEPROM(KI_GPS_PITCH, KI_GPS_PITCH_ADR); - writeEEPROM(GPS_MAX_ANGLE, GPS_MAX_ANGLE_ADR); - writeEEPROM(KP_ALTITUDE, KP_ALTITUDE_ADR); - writeEEPROM(KD_ALTITUDE, KD_ALTITUDE_ADR); - writeEEPROM(KI_ALTITUDE, KI_ALTITUDE_ADR); - writeEEPROM(acc_offset_x, acc_offset_x_ADR); - writeEEPROM(acc_offset_y, acc_offset_y_ADR); - writeEEPROM(acc_offset_z, acc_offset_z_ADR); - writeEEPROM(gyro_offset_roll, gyro_offset_roll_ADR); - writeEEPROM(gyro_offset_pitch, gyro_offset_pitch_ADR); - writeEEPROM(gyro_offset_yaw, gyro_offset_yaw_ADR); - writeEEPROM(Kp_ROLLPITCH, Kp_ROLLPITCH_ADR); - writeEEPROM(Ki_ROLLPITCH, Ki_ROLLPITCH_ADR); - writeEEPROM(Kp_YAW, Kp_YAW_ADR); - writeEEPROM(Ki_YAW, Ki_YAW_ADR); - writeEEPROM(GEOG_CORRECTION_FACTOR, GEOG_CORRECTION_FACTOR_ADR); - writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR); - writeEEPROM(Kp_RateRoll, KP_RATEROLL_ADR); - writeEEPROM(Ki_RateRoll, KI_RATEROLL_ADR); - writeEEPROM(Kd_RateRoll, KD_RATEROLL_ADR); - writeEEPROM(Kp_RatePitch, KP_RATEPITCH_ADR); - writeEEPROM(Ki_RatePitch, KI_RATEPITCH_ADR); - writeEEPROM(Kd_RatePitch, KD_RATEPITCH_ADR); - writeEEPROM(Kp_RateYaw, KP_RATEYAW_ADR); - writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR); - writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR); - writeEEPROM(xmitFactor, XMITFACTOR_ADR); - writeEEPROM(roll_mid, CHROLL_MID); - writeEEPROM(pitch_mid, CHPITCH_MID); - writeEEPROM(yaw_mid, CHYAW_MID); - writeEEPROM(ch_roll_slope, ch_roll_slope_ADR); - writeEEPROM(ch_pitch_slope, ch_pitch_slope_ADR); - writeEEPROM(ch_throttle_slope, ch_throttle_slope_ADR); - writeEEPROM(ch_yaw_slope, ch_yaw_slope_ADR); - writeEEPROM(ch_aux_slope, ch_aux_slope_ADR); - writeEEPROM(ch_aux2_slope, ch_aux2_slope_ADR); - writeEEPROM(ch_roll_offset, ch_roll_offset_ADR); - writeEEPROM(ch_pitch_offset, ch_pitch_offset_ADR); - writeEEPROM(ch_throttle_offset, ch_throttle_offset_ADR); - writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR); - writeEEPROM(ch_aux_offset, ch_aux_offset_ADR); - writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR); -}