mirror of https://github.com/ArduPilot/ardupilot
IMU performance enhancements and stable mode update
git-svn-id: https://arducopter.googlecode.com/svn/trunk@69 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
53a575c69c
commit
dfa605b00b
|
|
@ -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 <DataFlash.h>
|
||||
#include <APM_Compass.h>
|
||||
// Put your GPS library here:
|
||||
#include <GPS_NMEA.h> // MTK GPS
|
||||
//#include <GPS_NMEA.h> // MTK GPS
|
||||
#include <GPS_MTK.h>
|
||||
//#include <GPS_UBLOX.h>
|
||||
|
||||
// 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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -168,3 +162,65 @@ void Euler_angles(void)
|
|||
#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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue