cleaning up code and adding functions from APM main code

git-svn-id: https://arducopter.googlecode.com/svn/trunk@250 f9c3cf11-9bcb-44bc-f272-b75c42450872

Arducopter/ArduCopter.h

@@ -27,6 +27,12 @@
 // User configurable settings are on UserConfig.h
 /*******************************************************************/
 
+/**************************************************************/
+// Special features that might disapear in future releases
+
+//#define jpframe  // This is only Jani's special frame, you should never use unless you know what you are doing
+                 // As default this should be always checked off.
+
 
 /* APM Hardware definitions */
 #define LED_Yellow 36
@@ -48,12 +54,16 @@
 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
+
+#ifndef jpframe
 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}; 
  //{-1,1,-1,1,-1,1,-1,-1,-1};
-
+#else
+int SENSOR_SIGN[]={
+  1, -1, 1,    -1, 1, 1,     -1, -1, -1}; 
+ //{-1,1,-1,1,-1,1,-1,-1,-1};
+#endif
 
 /* APM Hardware definitions, END */
 
@@ -91,54 +101,42 @@ 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 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 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_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 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;
+float roll = 0;
+float pitch = 0;
+float yaw = 0;
 
-unsigned int counter=0;
+unsigned int counter = 0;
 
 float DCM_Matrix[3][3]= {
-  {
-    1,0,0  }
-  ,{
-    0,1,0  }
-  ,{
-    0,0,1  }
-}; 
+  { 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
+  { 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  }
-};
+  { 0,0,0 },
+  { 0,0,0 },
+  { 0,0,0 }};
 
 // GPS variables
 float speed_3d=0;
@@ -200,6 +198,30 @@ float command_altitude;
 float altitude_I;
 float altitude_D;
 
+//Pressure Sensor variables
+#ifdef UseBMP
+unsigned long abs_press 	        = 0;    
+unsigned long abs_press_filt            = 0;
+unsigned long abs_press_gnd             = 0;
+int 	ground_temperature	        = 0;    // 
+int 	temp_unfilt			= 0;
+long 	ground_alt			= 0;	// Ground altitude from gps at startup in centimeters
+long 	press_alt			= 0;	// Pressure altitude 
+#endif
+
+
+#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
+
+#define AIRSPEED_PIN 1		// Need to correct value
+#define BATTERY_PIN 1		// Need to correct value
+#define RELAY_PIN 47
+#define LOW_VOLTAGE	11.4    // Pack voltage at which to trigger alarm
+#define INPUT_VOLTAGE 5.2	// (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course)
+#define VOLT_DIV_RATIO 1.0	//  Voltage divider ratio set with thru-hole resistor (see manual)
+
+float 	battery_voltage 	= LOW_VOLTAGE * 1.05;		// Battery Voltage, initialized above threshold for filter
+
+
 // Sonar variables
 int Sonar_value=0;
 #define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
@@ -243,4 +265,4 @@ long tlmTimer = 0;
 
 // Arming/Disarming
 uint8_t Arming_counter=0;
-uint8_t Disarming_counter=0;
+uint8_t Disarming_counter=0;

Arducopter/Arducopter.pde

@@ -30,14 +30,20 @@
  GEAR OFF = Flight Assist (Stable Mode)
  
  **** LED Feedback ****
+ Bootup Sequence:
+ 1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery
+ 2) A, B, C LED's have running light while calibrating Gyro/Acc's
+ 3) Green LED Solid after initialization finished
+ 
  Green LED On = APM Initialization Finished
  Yellow LED On = GPS Hold Mode
  Yellow LED Off = Flight Assist Mode (No GPS)
  Red LED On = GPS Fix, 2D or 3D
  Red LED Off = No GPS Fix
-
+ 
  Green LED blink slow = Motors armed, Stable mode
  Green LED blink rapid = Motors armed, Acro mode 
+
 */
 
 /* User definable modules */
@@ -51,6 +57,18 @@
 
 #define CONFIGURATOR  // Do se use Configurator or normal text output over serial link
 
+
+
+/**********************************************/
+// Not in use yet, starting to work with battery monitors and pressure sensors. 
+// Added 19-08-2010
+
+//#define UseAirspeed
+//#define UseBMP
+//#define BATTERY_EVENT 1   // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!)
+
+/**********************************************/
+
 /* User definable modules - END */
 
 // Frame build condiguration
@@ -67,6 +85,9 @@
 #include <APM_RC.h>
 #include <DataFlash.h>
 #include <APM_Compass.h>
+#ifdef UseBMP
+#include <APM_BMP085.h>
+#endif
 
 //#include <GPS_NMEA.h>   // General NMEA GPS 
 #include <GPS_MTK.h>      // MediaTEK DIY Drones GPS. 
@@ -77,7 +98,8 @@
 #include "ArduCopter.h"
 #include "UserConfig.h"
 
-#define SWVER 1.31    // Current software version (only numeric values)
+/* Software version */
+#define VER 1.32    // Current software version (only numeric values)
 
 
 /* ***************************************************************************** */
@@ -85,7 +107,7 @@
 /* ***************************************************************************** */
 
 // Normal users does not need to edit anything below these lines. If you have
-// need, go and change them in FrameConfig.h
+// need, go and change them in UserConfig.h
 
 /* ************************************************************ */
 // STABLE MODE
@@ -127,14 +149,14 @@ void Attitude_control_v2()
   else
     err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch);  // Position Control
 
-  err_pitch = constrain(err_pitch,-25,25);  // to limit max pitch command...
+  err_pitch = constrain(err_pitch, -25, 25);  // to limit max pitch command...
 
   // New control term...
   pitch_rate = ToDeg(Omega[1]);
   err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
 
-  pitch_I += err_pitch*G_Dt;
-  pitch_I = constrain(pitch_I,-20,20);
+  pitch_I += err_pitch * G_Dt;
+  pitch_I = constrain(pitch_I, -20, 20);
   // D term
   pitch_D = - pitch_rate;
 
@@ -197,7 +219,7 @@ void Rate_control()
   err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
 
   yaw_I += err_yaw*G_Dt;
-  yaw_I = constrain(yaw_I,-20,20);
+  yaw_I = constrain(yaw_I, -20, 20);
 
   yaw_D = currentYawRate - previousYawRate;
   previousYawRate = currentYawRate;
@@ -215,15 +237,15 @@ int channel_filter(int ch, int ch_old)
   if (ch_old==0)      // ch_old not initialized
     return(ch);
   diff_ch_old = ch - ch_old;      // Difference with old reading
-  if (diff_ch_old<0)
+  if (diff_ch_old < 0)
   {
-    if (diff_ch_old<-40)
-      return(ch_old-40);        // We limit the max difference between readings
+    if (diff_ch_old <- 40)
+      return(ch_old - 40);        // We limit the max difference between readings
   }
   else
   {
-    if (diff_ch_old>40)    
-      return(ch_old+40);
+    if (diff_ch_old > 40)    
+      return(ch_old + 40);
   }
   return((ch + ch_old) >> 1);   // Small filtering
   //return(ch);
@@ -234,7 +256,7 @@ int channel_filter(int ch, int ch_old)
 /* ************************************************************ */
 void setup()
 {
-  int i;
+  int i, j;
   float aux_float[3];
 
   pinMode(LED_Yellow,OUTPUT); //Yellow LED A  (PC1)
@@ -246,7 +268,17 @@ void setup()
   pinMode(RELE_pin,OUTPUT);   // Rele output
   digitalWrite(RELE_pin,LOW);
 
-  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  //  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  for(i = 0; i <= 50; i++) {
+    digitalWrite(LED_Green, HIGH);
+    digitalWrite(LED_Yellow, HIGH);
+    digitalWrite(LED_Red, HIGH);
+    delay(20);
+    digitalWrite(LED_Green, LOW);
+    digitalWrite(LED_Yellow, LOW);
+    digitalWrite(LED_Red, LOW);
+    delay(20);
+  }
 
   APM_RC.Init();             // APM Radio initialization
   APM_ADC.Init();            // APM ADC library initialization
@@ -263,13 +295,12 @@ void setup()
 #endif
 
   readUserConfig(); // Load user configurable items from EEPROM
-  
+
   // Safety measure for Channel mids
   if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
   if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
   if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
 
-
   // RC channels Initialization (Quad motors)  
   APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
   APM_RC.OutputCh(1,MIN_THROTTLE);
@@ -306,6 +337,7 @@ void setup()
   aux_float[1] = gyro_offset_pitch;
   aux_float[2] = gyro_offset_yaw;
 
+  j = 0;
   // Take the gyro offset values
   for(i=0;i<300;i++)
   {
@@ -319,20 +351,42 @@ void setup()
     //Serial.println();
     Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
     delay(10);
+    
+    // Runnings lights effect to let user know that we are taking mesurements
+    if(j == 0) {
+      digitalWrite(LED_Green, HIGH);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, LOW);
+    } 
+    else if (j == 1) {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, HIGH);
+      digitalWrite(LED_Red, LOW);
+    } 
+    else {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, HIGH);
+    }
+    if((i % 5) == 0) j++;
+    if(j >= 3) j = 0;
   }
+  digitalWrite(LED_Green, LOW);
+  digitalWrite(LED_Yellow, LOW);
+  digitalWrite(LED_Red, LOW);
+
   for(int y=0; y<=2; y++)   
     AN_OFFSET[y]=aux_float[y];
 
-//  Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
+  //  Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
 #ifndef CONFIGURATOR
   for(i=0;i<6;i++)
   {
     Serial.print("AN[]:");
     Serial.println(AN_OFFSET[i]);
   }
-
   Serial.print("Yaw neutral value:");
-//  Serial.println(Neutro_yaw);
+  //  Serial.println(Neutro_yaw);
   Serial.print(yaw_mid);
 #endif
 
@@ -366,11 +420,13 @@ void setup()
   tlmTimer = millis();
   Read_adc_raw();        // Initialize ADC readings...
   delay(20);
-  
-    // Switch Left & Right lights on
+
+#ifdef IsAM
+  // Switch Left & Right lights on
   digitalWrite(RI_LED, HIGH);
   digitalWrite(LE_LED, HIGH); 
-  
+#endif
+
   motorArmed = 0;
   digitalWrite(LED_Green,HIGH);     // Ready to go...
 }
@@ -384,12 +440,12 @@ void loop(){
   int aux;
   int i;
   float aux_float;
+
   //Log variables
   int log_roll;
   int log_pitch;
   int log_yaw;
 
-
   if((millis()-timer)>=10)   // Main loop 100Hz
   {
     counter++;
@@ -409,12 +465,13 @@ void loop(){
       }
     }
 #endif    
+
     Matrix_update(); 
     Normalize();
     Drift_correction();
     Euler_angles();
-    // *****************
 
+    // *****************
     // Output data
     log_roll = ToDeg(roll) * 10;
     log_pitch = ToDeg(pitch) * 10;
@@ -427,7 +484,7 @@ void loop(){
     Serial.print(",");
     Serial.print(log_yaw);
 
-    for (int i=0;i<6;i++)
+    for (int i = 0; i < 6; i++)
     {
       Serial.print(AN[i]);
       Serial.print(",");
@@ -439,7 +496,7 @@ void loop(){
     // Write attitude to DataFlash log
     Log_Write_Attitude(log_roll,log_pitch,log_yaw);
 
-    if (APM_RC.GetState()==1)   // New radio frame?
+    if (APM_RC.GetState() == 1)   // New radio frame?
     {
       // Commands from radio Rx... 
       // Stick position defines the desired angle in roll, pitch and yaw
@@ -455,7 +512,7 @@ void loop(){
       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;
+      //      aux_float = (ch_yaw-Neutro_yaw) / 180.0;
       aux_float = (ch_yaw-yaw_mid) / 180.0;
       command_rx_yaw += aux_float;
       command_rx_yaw_diff = aux_float;
@@ -604,8 +661,9 @@ void loop(){
     // Quadcopter mix
     // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
     if (motorArmed == 1) {   
-       digitalWrite(FR_LED, HIGH);    // AM-Mode
-     
+#ifdef IsAM
+      digitalWrite(FR_LED, HIGH);    // AM-Mode
+#endif
 #ifdef FLIGHT_MODE_+
       rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
       leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
@@ -620,9 +678,11 @@ void loop(){
 #endif
     }
     if (motorArmed == 0) {
+#ifdef IsAM
       digitalWrite(FR_LED, LOW);    // AM-Mode
+#endif
       digitalWrite(LED_Green,HIGH); // Ready LED on
-      
+
       rightMotor = MIN_THROTTLE;
       leftMotor = MIN_THROTTLE;
       frontMotor = MIN_THROTTLE;
@@ -638,8 +698,8 @@ void loop(){
     APM_RC.OutputCh(1, leftMotor);    // Left motor
     APM_RC.OutputCh(2, frontMotor);   // Front motor
     APM_RC.OutputCh(3, backMotor);   // Back motor     
-    
-    // InstantPWM
+
+      // InstantPWM
     APM_RC.Force_Out0_Out1();
     APM_RC.Force_Out2_Out3();
 
@@ -674,7 +734,10 @@ void loop(){
     } 
   }
 
+} // End of void loop()
+
+
+// END of Arducopter.pde
+
 
 
-}
-

Arducopter/Functions.pde

@@ -135,11 +135,17 @@ void comma() {
   Serial.print(',');
 }
 
+#if BATTERY_EVENT == 1
+void low_battery_event(void)
+{
+//	send_message(SEVERITY_HIGH,"Low Battery!");
+//	set_mode(RTL);
+//	throttle_cruise = THROTTLE_CRUISE;
+}
+#endif
 
 
 
 
 
 
-
-

Arducopter/Navigation.pde

@@ -33,27 +33,27 @@ void Position_control(long lat_dest, long lon_dest)
   // ROLL
   gps_err_roll_old = gps_err_roll;
   //Optimization : cos(yaw) = DCM_Matrix[0][0] ;  sin(yaw) = DCM_Matrix[1][0] 
-  gps_err_roll = (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[0][0] - (float)Lat_diff*DCM_Matrix[1][0];
+  gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
 
-  gps_roll_D = (gps_err_roll-gps_err_roll_old)/GPS_Dt;
+  gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
 
-  gps_roll_I += gps_err_roll*GPS_Dt;
-  gps_roll_I = constrain(gps_roll_I,-800,800);
+  gps_roll_I += gps_err_roll * GPS_Dt;
+  gps_roll_I = constrain(gps_roll_I, -800, 800);
 
-  command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I;
-  command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
+  command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
+  command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
   
   // PITCH
   gps_err_pitch_old = gps_err_pitch;
-  gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0];
+  gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0];
 
-  gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/GPS_Dt;
+  gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt;
 
-  gps_pitch_I += gps_err_pitch*GPS_Dt;
-  gps_pitch_I = constrain(gps_pitch_I,-800,800);
+  gps_pitch_I += gps_err_pitch * GPS_Dt;
+  gps_pitch_I = constrain(gps_pitch_I, -800, 800);
 
-  command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I;
-  command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
+  command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I;
+  command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
 }
 
 /* ************************************************************ */
@@ -68,4 +68,4 @@ void Altitude_control(int target_sonar_altitude)
   command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
 }
 
-
+

Arducopter/SerialCom.pde

@@ -375,7 +375,7 @@ void sendSerialTelemetry() {
   case 'X': // Stop sending messages
     break;
   case '!': // Send flight software version
-    Serial.println(SWVER);
+    Serial.println(VER);
     queryType = 'X';
     break;
   case '.': // Modify GPS settings
@@ -403,4 +403,4 @@ float readFloatSerial() {
   }  
   while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
   return atof(data);
-}
+}

Arducopter/UserConfig.h

@@ -25,13 +25,14 @@ TODO:
 
 ************************************************************* */
 
+                 
 
 /*************************************************************/
 // Safety & Security 
 
 // Arm & Disarm delays
-#define ARM_DELAY 200
-#define DISARM_DELAY 100
+#define ARM_DELAY 200      // milliseconds of how long you need to keep rudder to max right for arming motors
+#define DISARM_DELAY 100   // milliseconds of how long you need to keep rudder to max left for disarming motors
 
 
 /*************************************************************/
@@ -57,10 +58,10 @@ TODO:
 
 //#define RADIO_TEST_MODE
 
-#define ROLL_MID 1478    // Radio Roll channel mid value
-#define PITCH_MID 1483    // Radio Pitch channel mid value
+#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 1502    // Radio Throttle channel mid value
+#define THROTTLE_MID 1505    // Radio Throttle channel mid value
 #define AUX_MID 1500
 
 #define CHANN_CENTER 1500       // Channel center, legacy
@@ -68,25 +69,25 @@ TODO:
 
 // Following variables stored in EEPROM
 float KP_QUAD_ROLL;
-float KD_QUAD_ROLL;
 float KI_QUAD_ROLL;
+float KD_QUAD_ROLL;
 float KP_QUAD_PITCH;
-float KD_QUAD_PITCH;
 float KI_QUAD_PITCH;
+float KD_QUAD_PITCH;
 float KP_QUAD_YAW;
-float KD_QUAD_YAW;
 float KI_QUAD_YAW;
+float KD_QUAD_YAW;
 float STABLE_MODE_KP_RATE;
 float KP_GPS_ROLL;
-float KD_GPS_ROLL;
 float KI_GPS_ROLL;
+float KD_GPS_ROLL;
 float KP_GPS_PITCH;
-float KD_GPS_PITCH;
 float KI_GPS_PITCH;
+float KD_GPS_PITCH;
 float GPS_MAX_ANGLE;
 float KP_ALTITUDE;
-float KD_ALTITUDE;
 float KI_ALTITUDE;
+float KD_ALTITUDE;
 int acc_offset_x;
 int acc_offset_y;
 int acc_offset_z;
@@ -126,25 +127,25 @@ float ch_aux2_offset = 0;
 // when a "Default EEPROM Value" command is sent through serial interface
 void setUserConfig() {
   KP_QUAD_ROLL = 1.8;
-  KD_QUAD_ROLL = 0.4; //0.48
   KI_QUAD_ROLL = 0.30; //0.4
+  KD_QUAD_ROLL = 0.4; //0.48
   KP_QUAD_PITCH = 1.8;
-  KD_QUAD_PITCH = 0.4; //0.48
   KI_QUAD_PITCH = 0.30; //0.4
+  KD_QUAD_PITCH = 0.4; //0.48
   KP_QUAD_YAW = 3.6;
-  KD_QUAD_YAW = 1.2;
   KI_QUAD_YAW = 0.15;
+  KD_QUAD_YAW = 1.2;
   STABLE_MODE_KP_RATE = 0.2;  // New param for stable mode
-  KP_GPS_ROLL = 0.012;
-  KD_GPS_ROLL = 0.005;
-  KI_GPS_ROLL = 0.004;
-  KP_GPS_PITCH = 0.012;
-  KD_GPS_PITCH = 0.005;
-  KI_GPS_PITCH = 0.004;
-  GPS_MAX_ANGLE = 10;
+  KP_GPS_ROLL = 0.02;
+  KI_GPS_ROLL = 0.008;
+  KD_GPS_ROLL = 0.006;
+  KP_GPS_PITCH = 0.02;
+  KI_GPS_PITCH = 0.008;
+  KD_GPS_PITCH = 0.006;
+  GPS_MAX_ANGLE = 18;
   KP_ALTITUDE = 0.8;
-  KD_ALTITUDE = 0.2;
   KI_ALTITUDE = 0.2;
+  KD_ALTITUDE = 0.2;
   acc_offset_x = 2073;
   acc_offset_y = 2056;
   acc_offset_z = 2010;
@@ -186,25 +187,25 @@ void setUserConfig() {
 
 // EEPROM storage addresses
 #define KP_QUAD_ROLL_ADR 0
-#define KD_QUAD_ROLL_ADR 4
 #define KI_QUAD_ROLL_ADR 8
+#define KD_QUAD_ROLL_ADR 4
 #define KP_QUAD_PITCH_ADR 12
-#define KD_QUAD_PITCH_ADR 16
 #define KI_QUAD_PITCH_ADR 20
+#define KD_QUAD_PITCH_ADR 16
 #define KP_QUAD_YAW_ADR 24
-#define KD_QUAD_YAW_ADR 28
 #define KI_QUAD_YAW_ADR 32
+#define KD_QUAD_YAW_ADR 28
 #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 KD_GPS_ROLL_ADR 44
 #define KP_GPS_PITCH_ADR 52
-#define KD_GPS_PITCH_ADR 56
 #define KI_GPS_PITCH_ADR 60
+#define KD_GPS_PITCH_ADR 56
 #define GPS_MAX_ANGLE_ADR 64
 #define KP_ALTITUDE_ADR 68
-#define KD_ALTITUDE_ADR 72
 #define KI_ALTITUDE_ADR 76
+#define KD_ALTITUDE_ADR 72
 #define acc_offset_x_ADR 80
 #define acc_offset_y_ADR 84
 #define acc_offset_z_ADR 88
@@ -268,25 +269,25 @@ void writeEEPROM(float value, int address) {
 
 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);
+  KD_QUAD_ROLL = readEEPROM(KD_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);
+  KD_QUAD_PITCH = readEEPROM(KD_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);
+  KD_QUAD_YAW = readEEPROM(KD_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);
+  KD_GPS_ROLL = readEEPROM(KD_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);
+  KD_GPS_PITCH = readEEPROM(KD_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);
+  KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
   acc_offset_x = readEEPROM(acc_offset_x_ADR);
   acc_offset_y = readEEPROM(acc_offset_y_ADR);
   acc_offset_z = readEEPROM(acc_offset_z_ADR);
@@ -326,4 +327,4 @@ void readUserConfig() {
   ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
   ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR); */
 }
-
+