mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
removed legacy_do_not_use directory and contents as it's just messy to have it hanging around. older versions can be retrieved from SVN if need be.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1904 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
588599d841
commit
60a8f1dab7
@ -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;
|
||||
}
|
||||
@ -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
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user