Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1904 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2011-04-18 02:13:57 +00:00
parent f652c5ae91
commit 31bf5a1f5d
13 changed files with 0 additions and 3257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

359
legacy_do_not_use/APM_Compass/APM_Compass.cpp
View File

@ -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 <math.h>
#include "WConstants.h"
}
#include <Wire.h>
#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;
}

113
legacy_do_not_use/APM_Compass/APM_Compass.h
View File

@ -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

88
legacy_do_not_use/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde
View File

@ -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 <Wire.h>
#include <APM_Compass.h> // Compass Library
#include <AP_Math.h> // 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();
}
}

19
legacy_do_not_use/APM_Compass/keywords.txt
View File

@ -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

253
legacy_do_not_use/Arducopter/ArduCopter.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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;

821
legacy_do_not_use/Arducopter/Arducopter.pde
View File

@ -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 <Wire.h>
#include <APM_ADC.h>
APM_ADC_Class APM_ADC;
#include <APM_RC.h>
#include <DataFlash.h>
#include <APM_Compass.h>
APM_Compass_Class APM_Compass;
#include <AP_Math.h>
#ifdef UseBMP
#include <APM_BMP085.h>
APM_BMP085_Class APM_BMP085;
#endif
#include <GPS_NMEA.h> // General NMEA GPS
//#include <GPS_MTK.h> // MediaTEK DIY Drones GPS.
//#include <GPS_UBLOX.h> // uBlox GPS
// EEPROM storage for user configurable values
#include <EEPROM.h>
#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

247
legacy_do_not_use/Arducopter/DCM.pde
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/* ******* 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];
}
}
}

151
legacy_do_not_use/Arducopter/Functions.pde
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
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

267
legacy_do_not_use/Arducopter/Log.pde
View File

@ -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);
}

73
legacy_do_not_use/Arducopter/Navigation.pde
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/* ************************************************************ */
/* 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;
}

72
legacy_do_not_use/Arducopter/Sensors.pde
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
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

395
legacy_do_not_use/Arducopter/SerialCom.pde
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
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);
}

399
legacy_do_not_use/Arducopter/UserConfig.h
View File

@ -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 <http://www.gnu.org/licenses/>.
* ************************************************************** *
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);
}