diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h new file mode 100644 index 0000000000..9e483a6a11 --- /dev/null +++ b/Arducopter/ArduCopter.h @@ -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 . +*/ + +#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); +} diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index a229767556..8ff478a7a5 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -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 #include #include #include #include -// Put your GPS library here: -//#include // MTK GPS -#include -//#include + +//#include // General NMEA GPS +#include // MediaTEK DIY Drones GPS. +//#include // uBlox GPS // EEPROM storage for user configurable values #include -#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) */ @@ -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) 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() 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() // 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() // 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() // 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() 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() // 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() // 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) 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() 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() 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(); @@ -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(){ { // 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(){ // 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,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(){ 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; } } diff --git a/Arducopter/DCM.pde b/Arducopter/DCM.pde index c96ac79f26..927dc67cb3 100644 --- a/Arducopter/DCM.pde +++ b/Arducopter/DCM.pde @@ -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 . + */ + + /* ******* 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]) } } - + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 27203ad4ed..2739898381 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -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 . -*/ + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ void readSerialCommand() { // Check for serial message @@ -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() { 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(','); -} +} + diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h new file mode 100644 index 0000000000..f52a50b337 --- /dev/null +++ b/Arducopter/UserConfig.h @@ -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 . + */ + +/* ************************************************************* +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... + diff --git a/Arducopter/UserSettings.h b/Arducopter/UserSettings.h deleted file mode 100644 index bad2ac7828..0000000000 --- a/Arducopter/UserSettings.h +++ /dev/null @@ -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 . -*/ - -#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); -}