From 4af9be6e8516f6910468493085e2032e783b6969 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Tue, 10 Aug 2010 21:06:01 +0000 Subject: [PATCH] New stable mode parameter compatible with configurator git-svn-id: https://arducopter.googlecode.com/svn/trunk@70 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/Arducopter.pde | 73 +++------------------------------------ Arducopter/SerialCom.pde | 8 ++--- Arducopter/UserSettings.h | 6 ++-- 3 files changed, 11 insertions(+), 76 deletions(-) diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index 93b0658250..8be506b3a4 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -277,77 +277,12 @@ 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... -// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands -void Attitude_control() -{ - // ROLL CONTROL - 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... - - 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... - // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected - // 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 = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command) - - // 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; - - // PITCH CONTROL - if (AP_mode==2) // Normal mode => Stabilization mode - err_pitch = command_rx_pitch - ToDeg(pitch); - 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... - - pitch_I += err_pitch*G_Dt; - pitch_I = constrain(pitch_I,-20,20); - // D term - pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]); - - // 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; - - // YAW CONTROL - - err_yaw = command_rx_yaw - ToDeg(yaw); - if (err_yaw > 180) // Normalize to -180,180 - 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 = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]); - - // PID control - control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; -} - /* ************************************************************ */ // STABLE MODE // ROLL, PITCH and YAW PID controls... // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands void Attitude_control_v2() { - - #define STABLE_MODE_KP_RATE_ROLL 0.2 - #define STABLE_MODE_KP_RATE_PITCH 0.2 - float err_roll_rate; float err_pitch_rate; float roll_rate; @@ -374,7 +309,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_ROLL*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 @@ -395,7 +330,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_PITCH*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); @@ -408,7 +343,7 @@ void Attitude_control_v2() yaw_I += err_yaw*G_Dt; yaw_I = constrain(yaw_I,-20,20); - yaw_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]); + yaw_D = - ToDeg(Omega[2]); // PID control control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; @@ -559,7 +494,7 @@ void setup() } //Serial.println(); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); - delay(14); + delay(10); } for(int y=0; y<=2; y++) AN_OFFSET[y]=aux_float[y]; diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 06b3c29f8b..27203ad4ed 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -33,7 +33,7 @@ void readSerialCommand() { KP_QUAD_YAW = readFloatSerial(); KI_QUAD_YAW = readFloatSerial(); KD_QUAD_YAW = readFloatSerial(); - KD_QUAD_COMMAND_PART = readFloatSerial(); + STABLE_MODE_KP_RATE = readFloatSerial(); MAGNETOMETER = readFloatSerial(); break; case 'C': // Receive GPS PID @@ -96,7 +96,7 @@ void readSerialCommand() { 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(KD_QUAD_COMMAND_PART, KD_QUAD_COMMAND_PART_ADR); + writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR); writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR); writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR); writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR); @@ -140,7 +140,7 @@ void readSerialCommand() { KP_QUAD_YAW = 3.6; KD_QUAD_YAW = 1.2; KI_QUAD_YAW = 0.15; - KD_QUAD_COMMAND_PART = 0.0; + 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; @@ -217,7 +217,7 @@ void sendSerialTelemetry() { comma(); Serial.print(KD_QUAD_YAW, 3); comma(); - Serial.print(KD_QUAD_COMMAND_PART, 3); + Serial.print(STABLE_MODE_KP_RATE, 3); comma(); Serial.println(MAGNETOMETER, 3); queryType = 'X'; diff --git a/Arducopter/UserSettings.h b/Arducopter/UserSettings.h index d1aa89c175..bad2ac7828 100644 --- a/Arducopter/UserSettings.h +++ b/Arducopter/UserSettings.h @@ -30,7 +30,7 @@ float KI_QUAD_PITCH; float KP_QUAD_YAW; float KD_QUAD_YAW; float KI_QUAD_YAW; -float KD_QUAD_COMMAND_PART; +float STABLE_MODE_KP_RATE; float KP_GPS_ROLL; float KD_GPS_ROLL; float KI_GPS_ROLL; @@ -74,7 +74,7 @@ float xmitFactor; #define KP_QUAD_YAW_ADR 24 #define KD_QUAD_YAW_ADR 28 #define KI_QUAD_YAW_ADR 32 -#define KD_QUAD_COMMAND_PART_ADR 36 +#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 @@ -141,7 +141,7 @@ void readUserConfig() { 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_COMMAND_PART = readEEPROM(KD_QUAD_COMMAND_PART_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);