From dfa605b00b53355e4d660e72c36e88509ce42173 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Tue, 10 Aug 2010 20:36:05 +0000 Subject: [PATCH] IMU performance enhancements and stable mode update git-svn-id: https://arducopter.googlecode.com/svn/trunk@69 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/Arducopter.pde | 109 ++++++++++++++++++++++++++++++++------ Arducopter/DCM.pde | 96 ++++++++++++++++++++++++++------- Arducopter/SerialCom.pde | 16 +++--- Arducopter/Vector.pde | 60 --------------------- 4 files changed, 177 insertions(+), 104 deletions(-) delete mode 100644 Arducopter/Vector.pde diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index eee97f071b..93b0658250 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -9,7 +9,7 @@ /* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */ /* Jani Hirvinen, Ken McEwans, Roberto Navoni, */ /* Sandro Benigno, Chris Anderson */ -/* Date : 04-07-2010 */ +/* Date : 08-08-2010 */ /* Version : 1.3 beta */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Mounting position : RC connectors pointing backwards */ @@ -19,7 +19,7 @@ /* DataFlash : DataFlash log library */ /* APM_BMP085 : BMP085 barometer library */ /* APM_Compass : HMC5843 compass library [optional] */ -/* GPS_UBLOX or GPS_NMEA: GPS library [optional] */ +/* GPS_UBLOX or GPS_NMEA or GPS_MTK : GPS library [optional] */ /* ********************************************************************** */ /* @@ -43,7 +43,8 @@ Red LED Off = No GPS Fix #include #include // Put your GPS library here: -#include // MTK GPS +//#include // MTK GPS +#include //#include // EEPROM storage for user configurable values @@ -92,6 +93,8 @@ 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) @@ -275,10 +278,9 @@ void Position_control(long lat_dest, long lon_dest) } /* ************************************************************ */ +// STABLE MODE // ROLL, PITCH and YAW PID controls... // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands - -// Stable Mode void Attitude_control() { // ROLL CONTROL @@ -336,7 +338,83 @@ void Attitude_control() control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; } -// Acro Mode +/* ************************************************************ */ +// 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; + float pitch_rate; + + // 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... + + // 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; + + 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 + 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; ; + + // 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... + + // New control term... + pitch_rate = ToDeg(Omega[1]); + err_pitch_rate = ((ch_pitch-1500)>>1) - pitch_rate; + + pitch_I += err_pitch*G_Dt; + pitch_I = constrain(pitch_I,-20,20); + // D term + pitch_D = - pitch_rate; + + // 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; + + // 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; +} + +// ACRO MODE void Rate_control() { static float previousRollRate, previousPitchRate, previousYawRate; @@ -358,8 +436,7 @@ void Rate_control() // PITCH CONTROL currentPitchRate = read_adc(1); - // err_pitch = ((1500-ch_pitch) * xmitFactor) - currentPitchRate; // was incorrect, inverted ELE between Arco / Stable - err_pitch = ((ch_pitch - 1500) * xmitFactor) - currentPitchRate; // correct one, now ELE is ok on both modes + err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate; pitch_I += err_pitch*G_Dt; pitch_I = constrain(pitch_I,-20,20); @@ -403,8 +480,8 @@ 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); + return((ch+ch_old)>>1); // Small filtering + //return(ch); } @@ -588,7 +665,7 @@ void loop(){ #endif // Write Sensor raw data to DataFlash log - Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); + Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp); // Write attitude to DataFlash log Log_Write_Attitude(log_roll,log_pitch,log_yaw); @@ -675,9 +752,9 @@ void loop(){ //Output GPS data //Serial.print(","); - Serial.print(GPS.Lattitude); - Serial.print(","); - Serial.print(GPS.Longitude); + //Serial.print(GPS.Lattitude); + //Serial.print(","); + //Serial.print(GPS.Longitude); // 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); @@ -704,7 +781,7 @@ void loop(){ // Control methodology selected using AUX2 if (ch_aux2 < 1200) - Attitude_control(); + Attitude_control_v2(); else { Rate_control(); @@ -775,4 +852,4 @@ void loop(){ tlmTimer = millis(); } #endif -} +} diff --git a/Arducopter/DCM.pde b/Arducopter/DCM.pde index 2553badcc0..c96ac79f26 100644 --- a/Arducopter/DCM.pde +++ b/Arducopter/DCM.pde @@ -8,7 +8,7 @@ void Read_adc_raw(void) AN[i] = APM_ADC.Ch(sensors[i]); // Correction for non ratiometric sensor (test code) - //temp = APM_ADC.Ch(3); + gyro_temp = APM_ADC.Ch(3); //AN[0] += 1500-temp; //AN[1] += 1500-temp; //AN[2] += 1500-temp; @@ -59,24 +59,18 @@ void Drift_correction(void) float errorCourse; static float Scaled_Omega_P[3]; static float Scaled_Omega_I[3]; - float Accel_magnitude; - float Accel_weight; //*****Roll and Pitch*************** - // Calculate the magnitude of the accelerometer vector - //Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); - //Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. - // Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0) - // Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); - // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - //Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); - Accel_weight = 1.0; - Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference - Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); + // errorRollPitch are in Accel ADC units + // Limit max errorRollPitch to limit max Omega_P and Omega_I + errorRollPitch[0] = constrain(errorRollPitch[0],-50,50); + errorRollPitch[1] = constrain(errorRollPitch[1],-50,50); + errorRollPitch[2] = constrain(errorRollPitch[2],-50,50); + Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH); - Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); + Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); //*****YAW*************** @@ -107,14 +101,14 @@ void Matrix_update(void) Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - Accel_Vector[0]=read_adc(3); // acc x - Accel_Vector[1]=read_adc(4); // acc y - Accel_Vector[2]=read_adc(5); // acc z + //Accel_Vector[0]=read_adc(3); // acc x + //Accel_Vector[1]=read_adc(4); // acc y + //Accel_Vector[2]=read_adc(5); // acc z // Low pass filter on accelerometer data (to filter vibrations) - //Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x - //Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y - //Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z + Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x + Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y + Accel_Vector[2]=Accel_Vector[2]*0.6 + (float)read_adc(5)*0.4; // acc z Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional @@ -167,4 +161,66 @@ void Euler_angles(void) yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); #endif } + + +// VECTOR FUNCTIONS +//Computes the dot product of two vectors +float Vector_Dot_Product(float vector1[3],float vector2[3]) +{ + float op=0; + + for(int c=0; c<3; c++) + { + op+=vector1[c]*vector2[c]; + } + + return op; +} + +//Computes the cross product of two vectors +void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) +{ + vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); + vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); + vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); +} + +//Multiply the vector by a scalar. +void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) +{ + for(int c=0; c<3; c++) + { + vectorOut[c]=vectorIn[c]*scale2; + } +} + +void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) +{ + for(int c=0; c<3; c++) + { + vectorOut[c]=vectorIn1[c]+vectorIn2[c]; + } +} + +/********* MATRIX FUNCTIONS *****************************************/ +//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). +void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) +{ + float op[3]; + for(int x=0; x<3; x++) + { + for(int y=0; y<3; y++) + { + for(int w=0; w<3; w++) + { + op[w]=a[x][w]*b[w][y]; + } + mat[x][y]=0; + mat[x][y]=op[0]+op[1]+op[2]; + + float test=mat[x][y]; + } + } +} + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 7cec5cd78a..06b3c29f8b 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -132,15 +132,15 @@ void readSerialCommand() { break; case 'Y': // Initialize EEPROM with default values KP_QUAD_ROLL = 1.8; - KD_QUAD_ROLL = 0.48; - KI_QUAD_ROLL = 0.40; + KD_QUAD_ROLL = 0.4; //0.48 + KI_QUAD_ROLL = 0.30; //0.4 KP_QUAD_PITCH = 1.8; - KD_QUAD_PITCH = 0.48; - KI_QUAD_PITCH = 0.40; + KD_QUAD_PITCH = 0.4; //0.48 + KI_QUAD_PITCH = 0.30; //0.4 KP_QUAD_YAW = 3.6; KD_QUAD_YAW = 1.2; KI_QUAD_YAW = 0.15; - KD_QUAD_COMMAND_PART = 4.0; + KD_QUAD_COMMAND_PART = 0.0; KP_GPS_ROLL = 0.012; KD_GPS_ROLL = 0.005; KI_GPS_ROLL = 0.004; @@ -157,9 +157,9 @@ void readSerialCommand() { gyro_offset_roll = 1659; gyro_offset_pitch = 1618; gyro_offset_yaw = 1673; - Kp_ROLLPITCH = 0.002; - Ki_ROLLPITCH = 0.0000005; - Kp_YAW = 1.5; + Kp_ROLLPITCH = 0.0014; + Ki_ROLLPITCH = 0.00000015; + Kp_YAW = 1.2; Ki_YAW = 0.00005; GEOG_CORRECTION_FACTOR = 0.87; MAGNETOMETER = 0; diff --git a/Arducopter/Vector.pde b/Arducopter/Vector.pde deleted file mode 100644 index 1986d03238..0000000000 --- a/Arducopter/Vector.pde +++ /dev/null @@ -1,60 +0,0 @@ -//Computes the dot product of two vectors -float Vector_Dot_Product(float vector1[3],float vector2[3]) -{ - float op=0; - - for(int c=0; c<3; c++) - { - op+=vector1[c]*vector2[c]; - } - - return op; -} - -//Computes the cross product of two vectors -void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) -{ - vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); - vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); - vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); -} - -//Multiply the vector by a scalar. -void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn[c]*scale2; - } -} - -void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn1[c]+vectorIn2[c]; - } -} - -/********* MATRIX FUNCTIONS *****************************************/ -//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) -{ - float op[3]; - for(int x=0; x<3; x++) - { - for(int y=0; y<3; y++) - { - for(int w=0; w<3; w++) - { - op[w]=a[x][w]*b[w][y]; - } - mat[x][y]=0; - mat[x][y]=op[0]+op[1]+op[2]; - - float test=mat[x][y]; - } - } -} - -