From d6fc9650a44b44b4b417fd100cb9f8a78bd45cef Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Thu, 2 Sep 2010 20:28:01 +0000 Subject: [PATCH] New Stable mode implementation. Fixed yaw issue. GPS position hold update. Acrobatic mode update to use bias corrected gyro rates. git-svn-id: https://arducopter.googlecode.com/svn/trunk@376 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/ArduCopter.h | 7 +- Arducopter/Arducopter.pde | 272 ++++++++++++++++++++------------------ Arducopter/Log.pde | 37 +++++- Arducopter/Navigation.pde | 14 +- Arducopter/SerialCom.pde | 16 +-- Arducopter/UserConfig.h | 86 ++++++------ 6 files changed, 237 insertions(+), 195 deletions(-) diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index 6a79b42d8e..8657b8f552 100644 --- a/Arducopter/ArduCopter.h +++ b/Arducopter/ArduCopter.h @@ -51,7 +51,7 @@ uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Ha // Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ int SENSOR_SIGN[]={ - 1, -1, 1, -1, 1, 1, -1, -1, -1}; + 1, -1, -1, -1, 1, 1, -1, -1, -1}; //{-1,1,-1,1,-1,1,-1,-1,-1}; /* APM Hardware definitions, END */ @@ -139,13 +139,8 @@ float GPS_Dt=0.2; // GPS Dt // 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; diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index 2bcb51153b..659ce0dcd3 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -49,7 +49,7 @@ /* User definable modules */ // Comment out with // modules that you are not using -//#define IsGPS // Do we have a GPS connected +#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 @@ -87,8 +87,8 @@ #include #endif -//#include // General NMEA GPS -#include // MediaTEK DIY Drones GPS. +#include // General NMEA GPS +//#include // MediaTEK DIY Drones GPS. //#include // uBlox GPS // EEPROM storage for user configurable values @@ -109,74 +109,64 @@ /* ************************************************************ */ // STABLE MODE -// ROLL, PITCH and YAW PID controls... +// PI absolute angle control driving a P rate control // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands -void Attitude_control_v2() +void Attitude_control_v3() { - float err_roll_rate; - float err_pitch_rate; - float roll_rate; - float pitch_rate; - + float stable_roll,stable_pitch,stable_yaw; + // 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 = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control + err_roll = constrain(err_roll,-25,25); // to limit max roll command... + + roll_I += err_roll*G_Dt; + roll_I = constrain(roll_I,-20,20); - 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 - roll_mid) >> 1) - roll_rate; - - 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 - roll_D = - roll_rate; // Take into account Angular velocity of the stick (command) - - // PID control + // PID absolute angle 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; - + stable_roll = K_aux*err_roll + KI_QUAD_ROLL*roll_I; + + // PD rate control (we use also the bias corrected gyro rates) + err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected + control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll; + // PITCH CONTROL if (AP_mode==2) // Normal mode => Stabilization mode err_pitch = command_rx_pitch - ToDeg(pitch); - else + else // GPS Position hold err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control - - 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); - // D term - pitch_D = - pitch_rate; - - // PID control + err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command... + + pitch_I += err_pitch*G_Dt; + pitch_I = constrain(pitch_I,-20,20); + + // PID absolute angle 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; - + stable_pitch = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I; + + // P rate control (we use also the bias corrected gyro rates) + err_pitch = stable_pitch - ToDeg(Omega[1]); + control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch; + // YAW CONTROL err_yaw = command_rx_yaw - ToDeg(yaw); if (err_yaw > 180) // Normalize to -180,180 - err_yaw -= 360; + err_yaw -= 360; else if(err_yaw < -180) err_yaw += 360; - - 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_D = - ToDeg(Omega[2]); - - // PID control - control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I; + 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); + + // PID absoulte angle control + stable_yaw = KP_QUAD_YAW*err_yaw + KI_QUAD_YAW*yaw_I; + // PD rate control (we use also the bias corrected gyro rates) + err_yaw = stable_yaw - ToDeg(Omega[2]); + control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw; } // ACRO MODE @@ -227,6 +217,55 @@ void Rate_control() control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; } +// RATE CONTROL MODE +// Using Omega vector (bias corrected gyro rate) +void Rate_control_v2() +{ + static float previousRollRate, previousPitchRate, previousYawRate; + float currentRollRate, currentPitchRate, currentYawRate; + + // ROLL CONTROL + currentRollRate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected + + err_roll = ((ch_roll- roll_mid) * xmitFactor) - currentRollRate; + + roll_I += err_roll*G_Dt; + roll_I = constrain(roll_I,-20,20); + + roll_D = (currentRollRate - previousRollRate)/G_Dt; + previousRollRate = currentRollRate; + + // PID control + control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; + + // PITCH CONTROL + currentPitchRate = ToDeg(Omega[1]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected + err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate; + + pitch_I += err_pitch*G_Dt; + pitch_I = constrain(pitch_I,-20,20); + + pitch_D = (currentPitchRate - previousPitchRate)/G_Dt; + previousPitchRate = currentPitchRate; + + // PID control + control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; + + // YAW CONTROL + currentYawRate = ToDeg(Omega[2]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected; + err_yaw = ((ch_yaw - yaw_mid)* xmitFactor) - currentYawRate; + + yaw_I += err_yaw*G_Dt; + yaw_I = constrain(yaw_I,-20,20); + + yaw_D = (currentYawRate - previousYawRate)/G_Dt; + previousYawRate = currentYawRate; + + // PID control + K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain + control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I; +} + // Maximun slope filter for radio inputs... (limit max differences between readings) int channel_filter(int ch, int ch_old) { @@ -237,13 +276,13 @@ int channel_filter(int ch, int ch_old) diff_ch_old = ch - ch_old; // Difference with old reading 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 <- 60) + return(ch_old - 60); // We limit the max difference between readings } else { - if (diff_ch_old > 40) - return(ch_old + 40); + if (diff_ch_old > 60) + return(ch_old + 60); } return((ch + ch_old) >> 1); // Small filtering //return(ch); @@ -265,6 +304,13 @@ void setup() pinMode(RELE_pin,OUTPUT); // Rele output digitalWrite(RELE_pin,LOW); + + APM_RC.Init(); // APM Radio initialization + // RC channels Initialization (Quad motors) + APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped + APM_RC.OutputCh(1,MIN_THROTTLE); + APM_RC.OutputCh(2,MIN_THROTTLE); + APM_RC.OutputCh(3,MIN_THROTTLE); // delay(1000); // Wait until frame is not moving after initial power cord has connected for(i = 0; i <= 50; i++) { @@ -278,7 +324,6 @@ void setup() delay(20); } - APM_RC.Init(); // APM Radio initialization APM_ADC.Init(); // APM ADC library initialization DataFlash.Init(); // DataFlash log initialization @@ -299,12 +344,6 @@ void setup() 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); - APM_RC.OutputCh(2,MIN_THROTTLE); - APM_RC.OutputCh(3,MIN_THROTTLE); - if (MAGNETOMETER == 1) APM_Compass.Init(); // I2C initialization @@ -320,12 +359,12 @@ void setup() while (digitalRead(SW1_pin)==0) { Serial.println("Entering Log Read Mode..."); - Log_Read(1,1000); + Log_Read(1,2000); delay(30000); } Read_adc_raw(); - delay(20); + delay(10); // Offset values for accels and gyros... AN_OFFSET[3] = acc_offset_x; @@ -387,30 +426,6 @@ void setup() // Serial.println(Neutro_yaw); Serial.print(yaw_mid); #endif - -#ifdef RADIO_TEST_MODE // RADIO TEST MODE TO TEST RADIO CHANNELS - while(1) - { - if (APM_RC.GetState()==1) - { - Serial.print("AIL:"); - Serial.print(APM_RC.InputCh(0)); - Serial.print("ELE:"); - Serial.print(APM_RC.InputCh(1)); - Serial.print("THR:"); - Serial.print(APM_RC.InputCh(2)); - Serial.print("YAW:"); - Serial.print(APM_RC.InputCh(3)); - Serial.print("AUX(mode):"); - Serial.print(APM_RC.InputCh(4)); - Serial.print("AUX2:"); - Serial.print(APM_RC.InputCh(5)); - Serial.println(); - delay(200); - } - } -#endif - delay(1000); DataFlash.StartWrite(1); // Start a write session on page 1 @@ -427,6 +442,7 @@ void setup() motorArmed = 0; digitalWrite(LED_Green,HIGH); // Ready to go... + } @@ -482,11 +498,11 @@ void loop(){ Serial.print(","); Serial.print(log_yaw); - for (int i = 0; i < 6; i++) - { - Serial.print(AN[i]); - Serial.print(","); - } + //for (int i = 0; i < 6; i++) + //{ + // Serial.print(AN[i]); + // Serial.print(","); + //} #endif // Write Sensor raw data to DataFlash log @@ -500,33 +516,30 @@ void loop(){ // Stick position defines the desired angle in roll, pitch and yaw ch_roll = channel_filter(APM_RC.InputCh(0) * ch_roll_slope + ch_roll_offset, ch_roll); ch_pitch = channel_filter(APM_RC.InputCh(1) * ch_pitch_slope + ch_pitch_offset, ch_pitch); - ch_throttle = channel_filter(APM_RC.InputCh(2) * ch_throttle_slope + ch_throttle_offset, ch_throttle); + //ch_throttle = channel_filter(APM_RC.InputCh(2) * ch_throttle_slope + ch_throttle_offset, ch_throttle); + ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle); // Transmiter calibration not used on throttle ch_yaw = channel_filter(APM_RC.InputCh(3) * ch_yaw_slope + ch_yaw_offset, ch_yaw); ch_aux = APM_RC.InputCh(4) * ch_aux_slope + ch_aux_offset; ch_aux2 = APM_RC.InputCh(5) * ch_aux2_slope + ch_aux2_offset; - 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_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-yaw_mid) / 180.0; + + command_rx_roll = (ch_roll-roll_mid) / 12.0; + command_rx_pitch = (ch_pitch-pitch_mid) / 12.0; + //aux_float = (ch_yaw-Neutro_yaw) / 180.0; + if (abs(ch_yaw-yaw_mid)<12) // Take into account a bit of "dead zone" on yaw + aux_float = 0.0; + else + aux_float = (ch_yaw-yaw_mid) / 180.0; command_rx_yaw += aux_float; - command_rx_yaw_diff = aux_float; if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees command_rx_yaw -= 360.0; else if (command_rx_yaw < -180) command_rx_yaw += 360.0; - + // 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-AUX_MID) / 300.0 + 1.7) * 0.2; // /300 + 1.0 - - if (K_aux < 0) - K_aux = 0; - + if (K_aux < 0) K_aux = 0; //Serial.print(","); //Serial.print(K_aux); @@ -590,7 +603,8 @@ 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 >= 2) + //if (GPS.Fix >= 2) + if (GPS.Fix) digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED else digitalWrite(LED_Red,LOW); @@ -613,27 +627,25 @@ void loop(){ // Control methodology selected using AUX2 if (ch_aux2 < 1200) { gled_speed = 1200; - Attitude_control_v2(); + Attitude_control_v3(); } else { gled_speed = 400; - Rate_control(); + Rate_control_v2(); // Reset yaw, so if we change to stable mode we continue with the actual yaw direction command_rx_yaw = ToDeg(yaw); - command_rx_yaw_diff = 0; } // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (ch_throttle < 1200) { + if (ch_throttle < (MIN_THROTTLE + 100)) { control_yaw = 0; command_rx_yaw = ToDeg(yaw); - command_rx_yaw_diff = 0; - if (ch_yaw < 1200) { + if (ch_yaw > 1850) { if (Arming_counter > ARM_DELAY){ if(ch_throttle > 800) { motorArmed = 1; - minThrottle = 1100; + minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle } } else @@ -642,7 +654,7 @@ void loop(){ else Arming_counter=0; // To Disarm motor output : Throttle down and full yaw left for more than 2 seconds - if (ch_yaw > 1800) { + if (ch_yaw < 1150) { if (Disarming_counter > DISARM_DELAY){ motorArmed = 0; minThrottle = MIN_THROTTLE; @@ -664,16 +676,16 @@ void loop(){ 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); - frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000); - backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000); + rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000); + leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000); + frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000); + backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000); #endif #ifdef FLIGHT_MODE_X - rightMotor = constrain(ch_throttle - control_roll + control_pitch - control_yaw, minThrottle, 2000); // front right motor - leftMotor = constrain(ch_throttle + control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear left motor - frontMotor = constrain(ch_throttle + control_roll + control_pitch + control_yaw, minThrottle, 2000); // front left motor - backMotor = constrain(ch_throttle - control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear right motor + rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor + leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor + frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor + backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor #endif } if (motorArmed == 0) { @@ -691,7 +703,6 @@ void loop(){ yaw_I = 0; // Initialize yaw command to actual yaw when throttle is down... command_rx_yaw = ToDeg(yaw); - command_rx_yaw_diff = 0; } APM_RC.OutputCh(0, rightMotor); // Right motor APM_RC.OutputCh(1, leftMotor); // Left motor @@ -735,8 +746,7 @@ void loop(){ } // End of void loop() - // END of Arducopter.pde - + diff --git a/Arducopter/Log.pde b/Arducopter/Log.pde index 3428ddd1ef..364a2b50ed 100644 --- a/Arducopter/Log.pde +++ b/Arducopter/Log.pde @@ -9,6 +9,7 @@ #define LOG_GPS_MSG 0x02 #define LOG_RADIO_MSG 0x03 #define LOG_SENSOR_MSG 0x04 +#define LOG_PID_MSG 0x05 // Write a Sensor Raw data packet void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7) @@ -38,6 +39,20 @@ void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw) DataFlash.WriteByte(END_BYTE); } +// Write a PID control info +void Log_Write_PID(byte num_PID, int P, int I,int D, int output) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_PID_MSG); + DataFlash.WriteByte(num_PID); + DataFlash.WriteInt(P); + DataFlash.WriteInt(I); + DataFlash.WriteInt(D); + DataFlash.WriteInt(output); + DataFlash.WriteByte(END_BYTE); +} + // Write an GPS packet. Total length : 30 bytes void Log_Write_GPS(long log_Time, long log_Lattitude, long log_Longitude, long log_Altitude, long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) @@ -112,6 +127,22 @@ void Log_Read_Attitude() Serial.println(); } +// Read a Sensor raw data packet +void Log_Read_PID() +{ + Serial.print("PID:"); + Serial.print((int)DataFlash.ReadByte()); // NUM_PID + Serial.print(","); + Serial.print(DataFlash.ReadInt()); // P + Serial.print(","); + Serial.print(DataFlash.ReadInt()); // I + Serial.print(","); + Serial.print(DataFlash.ReadInt()); // D + Serial.print(","); + Serial.print(DataFlash.ReadInt()); // output + Serial.println(); +} + // Read a GPS packet void Log_Read_GPS() { @@ -176,7 +207,7 @@ void Log_Read(int start_page, int end_page) { byte data; byte log_step=0; - int packet_count=0; + long packet_count=0; DataFlash.StartRead(start_page); while (DataFlash.GetPage() < end_page) @@ -211,6 +242,10 @@ void Log_Read(int start_page, int end_page) Log_Read_Sensor(); log_step++; break; + case LOG_PID_MSG: + Log_Read_PID(); + log_step++; + break; default: Serial.print("Error Reading Packet: "); Serial.print(packet_count); diff --git a/Arducopter/Navigation.pde b/Arducopter/Navigation.pde index d0961a73f6..489fe26e78 100644 --- a/Arducopter/Navigation.pde +++ b/Arducopter/Navigation.pde @@ -24,36 +24,38 @@ void Position_control(long lat_dest, long lon_dest) { long Lon_diff; long Lat_diff; - float gps_err_roll; - float gps_err_pitch; Lon_diff = lon_dest - GPS.Longitude; Lat_diff = lat_dest - GPS.Lattitude; // 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_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt; - + gps_err_roll_old = gps_err_roll; + 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 + //Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10); + // 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_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt; + gps_err_pitch_old = gps_err_pitch; 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 + + //Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10); } /* ************************************************************ */ @@ -68,4 +70,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 adc4715372..1d3ef91b78 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -26,14 +26,14 @@ void readSerialCommand() { case 'A': // Stable PID KP_QUAD_ROLL = readFloatSerial(); KI_QUAD_ROLL = readFloatSerial(); - KD_QUAD_ROLL = readFloatSerial(); + STABLE_MODE_KP_RATE_ROLL = readFloatSerial(); KP_QUAD_PITCH = readFloatSerial(); KI_QUAD_PITCH = readFloatSerial(); - KD_QUAD_PITCH = readFloatSerial(); + STABLE_MODE_KP_RATE_PITCH = readFloatSerial(); KP_QUAD_YAW = readFloatSerial(); KI_QUAD_YAW = readFloatSerial(); - KD_QUAD_YAW = readFloatSerial(); - STABLE_MODE_KP_RATE = readFloatSerial(); + STABLE_MODE_KP_RATE_YAW = readFloatSerial(); + STABLE_MODE_KP_RATE = readFloatSerial(); // NOT USED NOW MAGNETOMETER = readFloatSerial(); break; case 'C': // Receive GPS PID @@ -155,21 +155,21 @@ void sendSerialTelemetry() { comma(); Serial.print(KI_QUAD_ROLL, 3); comma(); - Serial.print(KD_QUAD_ROLL, 3); + Serial.print(STABLE_MODE_KP_RATE_ROLL, 3); comma(); Serial.print(KP_QUAD_PITCH, 3); comma(); Serial.print(KI_QUAD_PITCH, 3); comma(); - Serial.print(KD_QUAD_PITCH, 3); + Serial.print(STABLE_MODE_KP_RATE_PITCH, 3); comma(); Serial.print(KP_QUAD_YAW, 3); comma(); Serial.print(KI_QUAD_YAW, 3); comma(); - Serial.print(KD_QUAD_YAW, 3); + Serial.print(STABLE_MODE_KP_RATE_YAW, 3); comma(); - Serial.print(STABLE_MODE_KP_RATE, 3); + Serial.print(STABLE_MODE_KP_RATE, 3); // NOT USED NOW comma(); Serial.println(MAGNETOMETER, 3); queryType = 'X'; diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h index 5f46966e9e..d80fde8ef5 100644 --- a/Arducopter/UserConfig.h +++ b/Arducopter/UserConfig.h @@ -81,14 +81,14 @@ TODO: // Following variables stored in EEPROM float KP_QUAD_ROLL; float KI_QUAD_ROLL; -float KD_QUAD_ROLL; +float STABLE_MODE_KP_RATE_ROLL; float KP_QUAD_PITCH; float KI_QUAD_PITCH; -float KD_QUAD_PITCH; +float STABLE_MODE_KP_RATE_PITCH; float KP_QUAD_YAW; float KI_QUAD_YAW; -float KD_QUAD_YAW; -float STABLE_MODE_KP_RATE; +float STABLE_MODE_KP_RATE_YAW; +float STABLE_MODE_KP_RATE; // NOT USED NOW float KP_GPS_ROLL; float KI_GPS_ROLL; float KD_GPS_ROLL; @@ -137,23 +137,23 @@ float ch_aux2_offset = 0; // This function call contains the default values that are set to the ArduCopter // when a "Default EEPROM Value" command is sent through serial interface void defaultUserConfig() { - KP_QUAD_ROLL = 1.8; - KI_QUAD_ROLL = 0.30; //0.4 - KD_QUAD_ROLL = 0.4; //0.48 - KP_QUAD_PITCH = 1.8; - KI_QUAD_PITCH = 0.30; //0.4 - KD_QUAD_PITCH = 0.4; //0.48 - KP_QUAD_YAW = 3.6; + KP_QUAD_ROLL = 4.0; + KI_QUAD_ROLL = 0.15; + STABLE_MODE_KP_RATE_ROLL = 1.2; + KP_QUAD_PITCH = 4.0; + KI_QUAD_PITCH = 0.15; + STABLE_MODE_KP_RATE_PITCH = 1.2; + KP_QUAD_YAW = 3.0; 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.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; + STABLE_MODE_KP_RATE_YAW = 2.4; + STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW + KP_GPS_ROLL = 0.015; + KI_GPS_ROLL = 0.005; + KD_GPS_ROLL = 0.01; + KP_GPS_PITCH = 0.015; + KI_GPS_PITCH = 0.005; + KD_GPS_PITCH = 0.01; + GPS_MAX_ANGLE = 22; KP_ALTITUDE = 0.8; KI_ALTITUDE = 0.2; KD_ALTITUDE = 0.2; @@ -169,16 +169,16 @@ void defaultUserConfig() { Ki_YAW = 0.00005; GEOG_CORRECTION_FACTOR = 0.87; MAGNETOMETER = 0; - Kp_RateRoll = 0.6; - Ki_RateRoll = 0.1; - Kd_RateRoll = -0.8; - Kp_RatePitch = 0.6; - Ki_RatePitch = 0.1; - Kd_RatePitch = -0.8; - Kp_RateYaw = 1.6; - Ki_RateYaw = 0.3; - Kd_RateYaw = 0; - xmitFactor = 0.8; + Kp_RateRoll = 1.95; + Ki_RateRoll = 0.0; + Kd_RateRoll = 0.0; + Kp_RatePitch = 1.95; + Ki_RatePitch = 0.0; + Kd_RatePitch = 0.0; + Kp_RateYaw = 3.2; + Ki_RateYaw = 0.0; + Kd_RateYaw = 0.0; + xmitFactor = 0.32; roll_mid = 1500; pitch_mid = 1500; yaw_mid = 1500; @@ -199,14 +199,14 @@ void defaultUserConfig() { // EEPROM storage addresses #define KP_QUAD_ROLL_ADR 0 #define KI_QUAD_ROLL_ADR 8 -#define KD_QUAD_ROLL_ADR 4 +#define STABLE_MODE_KP_RATE_ROLL_ADR 4 #define KP_QUAD_PITCH_ADR 12 #define KI_QUAD_PITCH_ADR 20 -#define KD_QUAD_PITCH_ADR 16 +#define STABLE_MODE_KP_RATE_PITCH_ADR 16 #define KP_QUAD_YAW_ADR 24 #define KI_QUAD_YAW_ADR 32 -#define KD_QUAD_YAW_ADR 28 -#define STABLE_MODE_KP_RATE_ADR 36 +#define STABLE_MODE_KP_RATE_YAW_ADR 28 +#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW #define KP_GPS_ROLL_ADR 40 #define KI_GPS_ROLL_ADR 48 #define KD_GPS_ROLL_ADR 44 @@ -281,14 +281,14 @@ void writeEEPROM(float value, int address) { void readUserConfig() { KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); - KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR); + STABLE_MODE_KP_RATE_ROLL = readEEPROM(STABLE_MODE_KP_RATE_ROLL_ADR); KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); - KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR); + STABLE_MODE_KP_RATE_PITCH = readEEPROM(STABLE_MODE_KP_RATE_PITCH_ADR); KP_QUAD_YAW = readEEPROM(KP_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); + STABLE_MODE_KP_RATE_YAW = readEEPROM(STABLE_MODE_KP_RATE_YAW_ADR); + STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); // NOT USED NOW KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); @@ -340,15 +340,15 @@ void readUserConfig() { void writeUserConfig() { writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR); - writeEEPROM(KD_QUAD_ROLL, KD_QUAD_ROLL_ADR); writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR); + writeEEPROM(STABLE_MODE_KP_RATE_ROLL, STABLE_MODE_KP_RATE_ROLL_ADR); writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR); - writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR); writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR); + writeEEPROM(STABLE_MODE_KP_RATE_PITCH, STABLE_MODE_KP_RATE_PITCH_ADR); writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR); - writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR); writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR); - writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR); + writeEEPROM(STABLE_MODE_KP_RATE_YAW, STABLE_MODE_KP_RATE_YAW_ADR); + writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR); // NOT USED NOW writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR); writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR); writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR); @@ -396,4 +396,4 @@ void writeUserConfig() { writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR); writeEEPROM(ch_aux_offset, ch_aux_offset_ADR); writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR); -} +}