diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c index 26a038f46e..d091d82d61 100644 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.c +++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c @@ -1,4 +1,3 @@ - #include "std.h" #include "dcm.h" @@ -15,7 +14,7 @@ float G_Dt=0.05; float Gyro_Vector[3] = {0,0,0}; float Accel_Vector[3] = {0,0,0}; -float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data +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[3]= {0,0,0}; @@ -98,23 +97,23 @@ void Normalize(void) float temporary[3][3]; float renorm=0; boolean problem=FALSE; - + error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 - + Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 - + Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 - - renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); + + renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm= 1. / sqrt(renorm); -#if PERFORMANCE_REPORTING == 1 +#if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { @@ -124,13 +123,13 @@ void Normalize(void) #endif } Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); - - renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); + + renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { - renorm= 1. / sqrt(renorm); -#if PERFORMANCE_REPORTING == 1 + renorm= 1. / sqrt(renorm); +#if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { @@ -140,23 +139,23 @@ void Normalize(void) #endif } Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); - - renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); + + renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { - renorm= 1. / sqrt(renorm); -#if PERFORMANCE_REPORTING == 1 + renorm= 1. / sqrt(renorm); +#if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; #endif } else { - problem = TRUE; + problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif } Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); - + if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! DCM_Matrix[0][0]= 1.0f; DCM_Matrix[0][1]= 0.0f; @@ -167,7 +166,7 @@ void Normalize(void) DCM_Matrix[2][0]= 0.0f; DCM_Matrix[2][1]= 0.0f; DCM_Matrix[2][2]= 1.0f; - problem = FALSE; + problem = FALSE; } } @@ -184,7 +183,7 @@ float MAG_Heading; void Drift_correction(void) { - //Compensation the Roll, Pitch and Yaw drift. + //Compensation the Roll, Pitch and Yaw drift. static float Scaled_Omega_P[3]; static float Scaled_Omega_I[3]; float Accel_magnitude; @@ -212,37 +211,37 @@ void Drift_correction(void) Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. // Dynamic weighting of accelerometer info (reliability filter) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // - + Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // + #if PERFORMANCE_REPORTING == 1 { - + float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction imu_health += tempfloat; Bound(imu_health,129,65405); } #endif - + 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); - + Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); - + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); + //*****YAW*************** - - #if USE_MAGNETOMETER==1 + + #if USE_MAGNETOMETER==1 // We make the gyro YAW drift correction based on compass magnetic heading mag_heading_x = cos(MAG_Heading); mag_heading_y = sin(MAG_Heading); errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - + Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - + Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I #else // Use GPS Ground course to correct yaw gyro drift if(gps_mode==3 && ground_speed>= 0.5) //hwarm { @@ -251,12 +250,12 @@ void Drift_correction(void) COGY = sin(RadOfDeg(ground_course)); errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - + Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - + Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I } #endif // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros @@ -264,8 +263,8 @@ void Drift_correction(void) if (Integrator_magnitude > DegOfRad(300)) { Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude); } - - + + } /**************************************************/ @@ -277,11 +276,11 @@ void Matrix_update(void) if (gps_mode==3) //Remove centrifugal acceleration. { Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ - Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY } - - #if OUTPUTMODE==1 // With corrected data (drift correction) + + #if OUTPUTMODE==1 // With corrected data (drift correction) Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y @@ -310,7 +309,7 @@ void Matrix_update(void) for(int y=0; y<3; y++) { DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; - } + } } } @@ -327,4 +326,3 @@ void Euler_angles(void) euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif } - diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.h b/sw/airborne/subsystems/ahrs/dcm/dcm.h index 7c07e6fdef..74f862ddda 100644 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.h +++ b/sw/airborne/subsystems/ahrs/dcm/dcm.h @@ -1,4 +1,3 @@ - #include "math/pprz_algebra_float.h" // Inputs for DCM @@ -23,7 +22,7 @@ extern struct FloatEulers euler; //#define Kp_ROLLPITCH 0.2 #define Kp_ROLLPITCH 0.015 #define Ki_ROLLPITCH 0.000010 -#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution! +#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution! #define Ki_YAW 0.00005 #define GRAVITY 9.81 @@ -43,6 +42,3 @@ extern int renorm_sqrt_count; extern int renorm_blowup_count; extern float imu_health; #endif - - -