Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rearranging variables and new header files 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@138 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-08-16 01:10:21 +00:00
parent 4d1cbc1cca
commit a5b5692596
6 changed files with 655 additions and 484 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

399
Arducopter/ArduCopter.h Normal file
View File

@ -0,0 +1,399 @@
/*
ArduCopter 1.3 - Aug 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
/*******************************************************************/
/* 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, // GYROX, GYROY, GYROZ
-1, 1, 1, // ACCELX, ACCELY, ACCELZ
-1, -1, -1}; // MAGNETOX, MAGNETOY, MAGNETOZ
//{-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;
long timer=0; //general porpuse timer
long timer_old;
// Attitude control variables
float command_rx_roll=0; // User commands
float command_rx_roll_old;
float command_rx_roll_diff;
float command_rx_pitch=0;
float command_rx_pitch_old;
float command_rx_pitch_diff;
float command_rx_yaw=0;
float command_rx_yaw_diff;
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;
// 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;
// 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 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;
/*****************************************************/
// APM Specific Memory variables
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KD_QUAD_ROLL;
float KI_QUAD_ROLL;
float KP_QUAD_PITCH;
float KD_QUAD_PITCH;
float KI_QUAD_PITCH;
float KP_QUAD_YAW;
float KD_QUAD_YAW;
float KI_QUAD_YAW;
float STABLE_MODE_KP_RATE;
float KP_GPS_ROLL;
float KD_GPS_ROLL;
float KI_GPS_ROLL;
float KP_GPS_PITCH;
float KD_GPS_PITCH;
float KI_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KD_ALTITUDE;
float KI_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;
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KD_QUAD_ROLL_ADR 4
#define KI_QUAD_ROLL_ADR 8
#define KP_QUAD_PITCH_ADR 12
#define KD_QUAD_PITCH_ADR 16
#define KI_QUAD_PITCH_ADR 20
#define KP_QUAD_YAW_ADR 24
#define KD_QUAD_YAW_ADR 28
#define KI_QUAD_YAW_ADR 32
#define STABLE_MODE_KP_RATE_ADR 36
#define KP_GPS_ROLL_ADR 40
#define KD_GPS_ROLL_ADR 44
#define KI_GPS_ROLL_ADR 48
#define KP_GPS_PITCH_ADR 52
#define KD_GPS_PITCH_ADR 56
#define KI_GPS_PITCH_ADR 60
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KD_ALTITUDE_ADR 72
#define KI_ALTITUDE_ADR 76
#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
// 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);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
KI_ALTITUDE = readEEPROM(KI_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);
}

312
Arducopter/Arducopter.pde
View File

@ -1,5 +1,5 @@
/* ********************************************************************** */
/* ArduCopter Quadcopter code */
/* j ArduCopter Quadcopter code */
/* */
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
/* IMU DCM code from Diydrones.com */
@ -33,242 +33,60 @@
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
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 CONFIGURATOR // Do se use Configurator or normal text output over serial link
/* 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
/* ****************************************************************************** */
/* ****************************** Includes ************************************** */
/* ****************************************************************************** */
#include <Wire.h>
#include <APM_ADC.h>
#include <APM_RC.h>
#include <DataFlash.h>
#include <APM_Compass.h>
// Put your GPS library here:
//#include <GPS_NMEA.h> // MTK GPS
#include <GPS_MTK.h>
//#include <GPS_UBLOX.h>
//#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 "UserSettings.h"
#include "ArduCopter.h"
#include "UserConfig.h"
/* 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 SWVER 1.31 // Current software version (only numeric values)
/* AM PIN Definitions */
/* Can be changed in future to AN extension ports */
#define FR_LED 3 // Mega PE4 pin
#define RE_LED 2 // Mega PE5 pin
#define RI_LED 7 // Mega PH4 pin
#define LE_LED 8 // Mega PH5 pin
/* AM PIN Definitions - END */
/* ***************************************************************************** */
/* CONFIGURATION PART */
/* ************************ CONFIGURATION PART ********************************* */
/* ***************************************************************************** */
// 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
//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
int SENSOR_SIGN[]={
1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1};
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 Accel_magnitude;
//float Accel_weight;
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 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;
long timer=0; //general porpuse timer
long timer_old;
// Attitude control variables
float command_rx_roll=0; // User commands
float command_rx_roll_old;
float command_rx_roll_diff;
float command_rx_pitch=0;
float command_rx_pitch_old;
float command_rx_pitch_diff;
float command_rx_yaw=0;
float command_rx_yaw_diff;
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;
// 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;
// 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 Neutro_yaw;
int ch_roll;
int ch_pitch;
int ch_throttle;
int ch_yaw;
int ch_aux;
int ch_aux2;
#define CHANN_CENTER 1500
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
// Motor variables
#define FLIGHT_MODE_+
//#define FLIGHT_MODE_X
int frontMotor;
int backMotor;
int leftMotor;
int rightMotor;
byte motorArmed = 0;
int minThrottle = 0;
// Serial communication
#define CONFIGURATOR
char queryType;
long tlmTimer = 0;
// Arming/Disarming
uint8_t Arming_counter=0;
uint8_t Disarming_counter=0;
// Normal users does not need to edit anything below these lines. If you have
// need, go and change them in FrameConfig.h
/* ************************************************************ */
/* Altitude control... (based on sonar) */
@ -320,6 +138,8 @@ void Position_control(long lat_dest, long lon_dest)
command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
}
/* ************************************************************ */
// STABLE MODE
// ROLL, PITCH and YAW PID controls...
@ -341,7 +161,7 @@ void Attitude_control_v2()
// New control term...
roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
err_roll_rate = ((ch_roll-1500)>>1) - roll_rate;
err_roll_rate = ((ch_roll - ROLL_MID) >> 1) - roll_rate;
roll_I += err_roll * G_Dt;
roll_I = constrain(roll_I, -20, 20);
@ -353,7 +173,6 @@ void Attitude_control_v2()
// PID control
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
control_roll = K_aux * err_roll + KD_QUAD_ROLL * roll_D + KI_QUAD_ROLL * roll_I + STABLE_MODE_KP_RATE * err_roll_rate;
;
// PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode
@ -365,7 +184,7 @@ void Attitude_control_v2()
// New control term...
pitch_rate = ToDeg(Omega[1]);
err_pitch_rate = ((ch_pitch-1500)>>1) - pitch_rate;
err_pitch_rate = ((ch_pitch - PITCH_MID) >> 1) - pitch_rate;
pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20);
@ -402,7 +221,7 @@ void Rate_control()
// ROLL CONTROL
currentRollRate = read_adc(0); // I need a positive sign here
err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate;
err_roll = ((ch_roll - ROLL_MID) * xmitFactor) - currentRollRate;
roll_I += err_roll * G_Dt;
roll_I = constrain(roll_I, -20, 20);
@ -415,7 +234,7 @@ void Rate_control()
// PITCH CONTROL
currentPitchRate = read_adc(1);
err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate;
err_pitch = ((ch_pitch - PITCH_MID) * xmitFactor) - currentPitchRate;
pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20);
@ -428,7 +247,7 @@ void Rate_control()
// YAW CONTROL
currentYawRate = read_adc(2);
err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate;
err_yaw = ((ch_yaw - YAW_MID) * xmitFactor) - currentYawRate;
yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20);
@ -463,8 +282,9 @@ int channel_filter(int ch, int ch_old)
//return(ch);
}
/* ****** SETUP ********************************************************************* */
/* ************************************************************ */
/* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */
void setup()
{
int i;
@ -479,12 +299,21 @@ void setup()
pinMode(RELE_pin,OUTPUT); // Rele output
digitalWrite(RELE_pin,LOW);
delay(250);
delay(1000); // Wait until frame is not moving after initial power cord has connected
APM_RC.Init(); // APM Radio initialization
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
@ -513,7 +342,6 @@ void setup()
delay(30000);
}
//delay(3000);
Read_adc_raw();
delay(20);
@ -594,7 +422,10 @@ void setup()
digitalWrite(LED_Green,HIGH); // Ready to go...
}
/* ***** MAIN LOOP ***** */
/* ************************************************************ */
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
/* ************************************************************ */
void loop(){
int aux;
@ -615,6 +446,7 @@ void loop(){
// IMU DCM Algorithm
Read_adc_raw();
#ifdef IsMAG
if (MAGNETOMETER == 1) {
if (counter > 10) // Read compass data at 10Hz... (10 loop runs)
{
@ -623,6 +455,7 @@ void loop(){
APM_Compass.Calculate(roll,pitch); // Calculate heading
}
}
#endif
Matrix_update();
Normalize();
Drift_correction();
@ -680,7 +513,7 @@ void loop(){
// 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-1500)/300.0 + 1.7)*0.2; // /300 + 1.0
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;
@ -703,12 +536,14 @@ void loop(){
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
Serial.println();
Serial.print("* Target:");
@ -743,7 +578,7 @@ void loop(){
// 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)
if (GPS.Fix >= 2)
digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED
else
digitalWrite(LED_Red,LOW);
@ -783,7 +618,7 @@ void loop(){
command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0;
if (ch_yaw > 1800) {
if (Arming_counter>240){
if (Arming_counter > ARM_DELAY){
motorArmed = 1;
minThrottle = 1100;
}
@ -794,7 +629,7 @@ void loop(){
Arming_counter=0;
// To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
if (ch_yaw < 1200) {
if (Disarming_counter>240){
if (Disarming_counter > DISARM_DELAY){
motorArmed = 0;
minThrottle = MIN_THROTTLE;
}
@ -846,6 +681,7 @@ void loop(){
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();
@ -862,17 +698,21 @@ void loop(){
}
#endif
// AM and Mode lights
// 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;
}
}

21
Arducopter/DCM.pde
View File

@ -1,3 +1,24 @@
/*
ArduCopter v1.3 - Aug 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)

46
Arducopter/SerialCom.pde
View File

@ -182,19 +182,32 @@ void sendSerialTelemetry() {
float aux_float[3]; // used for sensor calibration
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
Serial.print("throttle =");Serial.println(ch_throttle);
Serial.print("control roll =");Serial.println(control_roll-CHANN_CENTER);
Serial.print("control pitch =");Serial.println(control_pitch-CHANN_CENTER);
Serial.print("control yaw =");Serial.println(control_yaw-CHANN_CENTER);
Serial.print("front left yaw =");Serial.println(frontMotor);
Serial.print("front right yaw =");Serial.println(rightMotor);
Serial.print("rear left yaw =");Serial.println(leftMotor);
Serial.print("rear right motor =");Serial.println(backMotor);Serial.println();
Serial.print("throttle =");
Serial.println(ch_throttle);
Serial.print("control roll =");
Serial.println(control_roll-CHANN_CENTER);
Serial.print("control pitch =");
Serial.println(control_pitch-CHANN_CENTER);
Serial.print("control yaw =");
Serial.println(control_yaw-CHANN_CENTER);
Serial.print("front left yaw =");
Serial.println(frontMotor);
Serial.print("front right yaw =");
Serial.println(rightMotor);
Serial.print("rear left yaw =");
Serial.println(leftMotor);
Serial.print("rear right motor =");
Serial.println(backMotor);
Serial.println();
Serial.print("current roll rate =");Serial.println(read_adc(0));
Serial.print("current pitch rate =");Serial.println(read_adc(1));
Serial.print("current yaw rate =");Serial.println(read_adc(2));
Serial.print("command rx yaw =");Serial.println(command_rx_yaw);
Serial.print("current roll rate =");
Serial.println(read_adc(0));
Serial.print("current pitch rate =");
Serial.println(read_adc(1));
Serial.print("current yaw rate =");
Serial.println(read_adc(2));
Serial.print("command rx yaw =");
Serial.println(command_rx_yaw);
Serial.println();
queryType = 'X';
break;
@ -379,9 +392,12 @@ void sendSerialTelemetry() {
case 'X': // Stop sending messages
break;
case '!': // Send flight software version
Serial.println("1.2");
Serial.println(SWVER);
queryType = 'X';
break;
case '.': // Modify GPS settings
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
break;
}
}
@ -401,10 +417,12 @@ float readFloatSerial() {
timeout = 0;
index++;
}
} while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
}
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data);
}
void comma() {
Serial.print(',');
}

70
Arducopter/UserConfig.h Normal file
View File

@ -0,0 +1,70 @@
/*
ArduCopter v1.3 - Aug 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/>.
*/
/* *************************************************************
TODO:
- move all user definable variables from main pde to here
- comment variables properly
************************************************************* */
/*************************************************************/
// Safety & Security
// Arm & Disarm delays
#define ARM_DELAY 200
#define DISARM_DELAY 100
/*************************************************************/
// 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 1500 // 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...

177
Arducopter/UserSettings.h
View File

@ -1,177 +0,0 @@
/*
ArduCopter v1.2
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"
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KD_QUAD_ROLL;
float KI_QUAD_ROLL;
float KP_QUAD_PITCH;
float KD_QUAD_PITCH;
float KI_QUAD_PITCH;
float KP_QUAD_YAW;
float KD_QUAD_YAW;
float KI_QUAD_YAW;
float STABLE_MODE_KP_RATE;
float KP_GPS_ROLL;
float KD_GPS_ROLL;
float KI_GPS_ROLL;
float KP_GPS_PITCH;
float KD_GPS_PITCH;
float KI_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KD_ALTITUDE;
float KI_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;
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KD_QUAD_ROLL_ADR 4
#define KI_QUAD_ROLL_ADR 8
#define KP_QUAD_PITCH_ADR 12
#define KD_QUAD_PITCH_ADR 16
#define KI_QUAD_PITCH_ADR 20
#define KP_QUAD_YAW_ADR 24
#define KD_QUAD_YAW_ADR 28
#define KI_QUAD_YAW_ADR 32
#define STABLE_MODE_KP_RATE_ADR 36
#define KP_GPS_ROLL_ADR 40
#define KD_GPS_ROLL_ADR 44
#define KI_GPS_ROLL_ADR 48
#define KP_GPS_PITCH_ADR 52
#define KD_GPS_PITCH_ADR 56
#define KI_GPS_PITCH_ADR 60
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KD_ALTITUDE_ADR 72
#define KI_ALTITUDE_ADR 76
#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
// 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);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
KI_ALTITUDE = readEEPROM(KI_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);
}