From 989874b7bf0f480433d57418ee8cee5d383f67bc Mon Sep 17 00:00:00 2001 From: Abel Tim Date: Wed, 3 Jul 2024 15:37:21 +0200 Subject: direction code and better IR seeker code --- .../MinIMU9AHRS/DCM.ino | 164 +++++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino (limited to 'debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino') diff --git a/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino new file mode 100644 index 0000000..7f9f276 --- /dev/null +++ b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino @@ -0,0 +1,164 @@ +/* + +MinIMU-9-Arduino-AHRS +Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) + +Copyright (c) 2011-2016 Pololu Corporation. +http://www.pololu.com/ + +MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: +http://code.google.com/p/sf9domahrs/ + +sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose +Julio and Doug Weibel: +http://code.google.com/p/ardu-imu/ + +MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it +under the terms of the GNU Lesser General Public License as published by the +Free Software Foundation, either version 3 of the License, or (at your option) +any later version. + +MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but +WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for +more details. + +You should have received a copy of the GNU Lesser General Public License along +with MinIMU-9-Arduino-AHRS. If not, see . + +*/ + +/**************************************************/ +void Normalize(void) +{ + float error=0; + float temporary[3][3]; + float renorm=0; + + 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= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 + Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); + + renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 + Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); + + renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 + Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); +} + +/**************************************************/ +void Drift_correction(void) +{ + float mag_heading_x; + float mag_heading_y; + float errorCourse; + //Compensation the Roll, Pitch and Yaw drift. + 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. + // Dynamic weighting of accelerometer info (reliability filter) + // 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); // + + 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); + + //*****YAW*************** + // 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);//.01proportional of YAW. + Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. + + Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I +} +/**************************************************/ +/* +void Accel_adjust(void) +{ + Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ + Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY +} +*/ +/**************************************************/ + +void Matrix_update(void) +{ + Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll + Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch + Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw + + Accel_Vector[0]=accel_x; + Accel_Vector[1]=accel_y; + Accel_Vector[2]=accel_z; + + Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term + Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term + + //Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement + + #if OUTPUTMODE==1 + 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 + Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x + Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y + Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x + Update_Matrix[2][2]=0; + #else // Uncorrected data (no drift correction) + Update_Matrix[0][0]=0; + Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z + Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y + Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; + Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; + Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; + Update_Matrix[2][2]=0; + #endif + + Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c + + for(int x=0; x<3; x++) //Matrix Addition (update) + { + for(int y=0; y<3; y++) + { + DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; + } + } +} + +void Euler_angles(void) +{ + pitch = -asin(DCM_Matrix[2][0]); + roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); +} + -- cgit v1.2.3