zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Rearranging variables and new header files 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@138 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
jphelirc 15 years ago
parent
4d1cbc1cca
commit
a5b5692596
6 changed files with 655 additions and 484 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 399
      Arducopter/ArduCopter.h
  2. 388
      Arducopter/Arducopter.pde
  3. 23
      Arducopter/DCM.pde
  4. 82
      Arducopter/SerialCom.pde
  5. 70
      Arducopter/UserConfig.h
  6. 177
      Arducopter/UserSettings.h

399
Arducopter/ArduCopter.h
Unescape View File

@ -0,0 +1,399 @@ @@ -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);
}

388
Arducopter/Arducopter.pde
Unescape View File

@ -1,5 +1,5 @@ @@ -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 @@ @@ -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"
/* 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
/* *** */
#include "ArduCopter.h"
#include "UserConfig.h"
/* AM PIN Definitions */
/* Can be changed in future to AN extension ports */
#define SWVER 1.31 // Current software version (only numeric values)
#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) */
@ -276,10 +94,10 @@ void Altitude_control(int target_sonar_altitude) @@ -276,10 +94,10 @@ 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;
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;
}
/* ************************************************************ */
@ -320,6 +138,8 @@ void Position_control(long lat_dest, long lon_dest) @@ -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...
@ -332,19 +152,19 @@ void Attitude_control_v2() @@ -332,19 +152,19 @@ void Attitude_control_v2()
float pitch_rate;
// ROLL CONTROL
if (AP_mode==2) // Normal Mode => Stabilization mode
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...
err_roll = constrain(err_roll, -25, 25); // to limit max roll command...
// 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);
roll_I += err_roll * G_Dt;
roll_I = constrain(roll_I, -20, 20);
// D term implementation => two parts: gyro part and command part
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
@ -352,8 +172,7 @@ void Attitude_control_v2() @@ -352,8 +172,7 @@ 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;
;
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() @@ -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);
@ -374,7 +193,7 @@ void Attitude_control_v2() @@ -374,7 +193,7 @@ void Attitude_control_v2()
// PID control
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE*err_pitch_rate;
control_pitch = K_aux * err_pitch + KD_QUAD_PITCH * pitch_D + KI_QUAD_PITCH * pitch_I + STABLE_MODE_KP_RATE * err_pitch_rate;
// YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw);
@ -383,14 +202,14 @@ void Attitude_control_v2() @@ -383,14 +202,14 @@ void Attitude_control_v2()
else if(err_yaw < -180)
err_yaw += 360;
err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command...
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);
yaw_I += err_yaw * G_Dt;
yaw_I = constrain(yaw_I, -20, 20);
yaw_D = - ToDeg(Omega[2]);
// PID control
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
}
// ACRO MODE
@ -402,20 +221,20 @@ void Rate_control() @@ -402,20 +221,20 @@ 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);
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;
control_roll = Kp_RateRoll * err_roll + Kd_RateRoll * roll_D + Ki_RateRoll * roll_I;
// 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() @@ -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);
@ -459,12 +278,13 @@ int channel_filter(int ch, int ch_old) @@ -459,12 +278,13 @@ int channel_filter(int ch, int ch_old)
if (diff_ch_old>40)
return(ch_old+40);
}
return((ch+ch_old)>>1); // Small filtering
return((ch + ch_old) >> 1); // Small filtering
//return(ch);
}
/* ****** SETUP ********************************************************************* */
/* ************************************************************ */
/* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */
void setup()
{
int i;
@ -474,17 +294,26 @@ void setup() @@ -474,17 +294,26 @@ void setup()
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(SW1_pin,INPUT); //Switch SW1 (pin PG0)
pinMode(RELE_pin,OUTPUT); // Rele output
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
GPS.Init(); // GPS Initialization
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() @@ -513,7 +342,6 @@ void setup()
delay(30000);
}
//delay(3000);
Read_adc_raw();
delay(20);
@ -594,7 +422,10 @@ void setup() @@ -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(){ @@ -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(){ @@ -623,6 +455,7 @@ void loop(){
APM_Compass.Calculate(roll,pitch); // Calculate heading
}
}
#endif
Matrix_update();
Normalize();
Drift_correction();
@ -630,9 +463,9 @@ void loop(){ @@ -630,9 +463,9 @@ void loop(){
// *****************
// Output data
log_roll = ToDeg(roll)*10;
log_pitch = ToDeg(pitch)*10;
log_yaw = ToDeg(yaw)*10;
log_roll = ToDeg(roll) * 10;
log_pitch = ToDeg(pitch) * 10;
log_yaw = ToDeg(yaw) * 10;
#ifndef CONFIGURATOR
Serial.print(log_roll);
@ -657,19 +490,19 @@ void loop(){ @@ -657,19 +490,19 @@ void loop(){
{
// 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);
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_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);
command_rx_roll_old = command_rx_roll;
command_rx_roll = (ch_roll-CHANN_CENTER)/12.0;
command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0;
command_rx_roll_diff = command_rx_roll - command_rx_roll_old;
command_rx_pitch_old = command_rx_pitch;
command_rx_pitch = (ch_pitch-CHANN_CENTER)/12.0;
command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old;
aux_float = (ch_yaw-Neutro_yaw)/180.0;
command_rx_pitch = (ch_pitch-CHANN_CENTER) / 12.0;
command_rx_pitch_diff = command_rx_pitch - command_rx_pitch_old;
aux_float = (ch_yaw-Neutro_yaw) / 180.0;
command_rx_yaw += aux_float;
command_rx_yaw_diff = aux_float;
if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees
@ -680,7 +513,7 @@ void loop(){ @@ -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(){ @@ -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,14 +578,14 @@ void loop(){ @@ -743,14 +578,14 @@ 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);
if (AP_mode==1)
{
if ((target_position==1)&&(GPS.Fix))
if ((target_position==1) && (GPS.Fix))
{
Position_control(target_lattitude,target_longitude); // Call position hold routine
}
@ -783,7 +618,7 @@ void loop(){ @@ -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(){ @@ -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(){ @@ -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(){ @@ -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;
}
}

23
Arducopter/DCM.pde
Unescape View File

@ -1,3 +1,24 @@ @@ -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)
@ -223,4 +244,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) @@ -223,4 +244,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
}
}

82
Arducopter/SerialCom.pde
Unescape View File

@ -1,22 +1,22 @@ @@ -1,22 +1,22 @@
/*
ArduCopter v1.2 - June 2010
www.ArduCopter.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
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/>.
*/
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
@ -182,19 +182,32 @@ void sendSerialTelemetry() { @@ -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("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("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.println();
queryType = 'X';
break;
@ -379,9 +392,12 @@ void sendSerialTelemetry() { @@ -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() { @@ -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
Unescape View File

@ -0,0 +1,70 @@ @@ -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
Unescape View File

@ -1,177 +0,0 @@ @@ -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);
}
Write Preview
Loading…
Cancel
Save