diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index adab275e7e..3b868264aa 100644 --- a/Arducopter/ArduCopter.h +++ b/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 -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}; +#ifndef jpframe +int SENSOR_SIGN[]={ + 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; diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index 42cc804592..fd60c19c9e 100644 --- a/Arducopter/Arducopter.pde +++ b/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 #include #include +#ifdef UseBMP +#include +#endif //#include // General NMEA GPS #include // 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 + -} - diff --git a/Arducopter/Functions.pde b/Arducopter/Functions.pde index c367d703a5..2797d15752 100644 --- a/Arducopter/Functions.pde +++ b/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 - - diff --git a/Arducopter/Navigation.pde b/Arducopter/Navigation.pde index 767daf85b2..d0961a73f6 100644 --- a/Arducopter/Navigation.pde +++ b/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; } - + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 8478f6def9..5394583a11 100644 --- a/Arducopter/SerialCom.pde +++ b/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); -} +} diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h index 25a325708b..5d96203904 100644 --- a/Arducopter/UserConfig.h +++ b/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); */ } - +