From 9c8f7e2f1101b4bb8e0c5459e14af253f150c8b7 Mon Sep 17 00:00:00 2001 From: Abel Tim Date: Mon, 17 Mar 2025 21:43:59 +0100 Subject: code --- .../MinIMU9AHRS/Compass.ino | 56 ----- .../MinIMU9AHRS/DCM.ino | 164 -------------- .../MinIMU9AHRS/I2C.ino | 159 -------------- .../MinIMU9AHRS/MinIMU9AHRS.ino | 241 --------------------- .../MinIMU9AHRS/Output.ino | 91 -------- .../MinIMU9AHRS/Vector.ino | 70 ------ .../MinIMU9AHRS/matrix.ino | 49 ----- 7 files changed, 830 deletions(-) delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino delete mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/matrix.ino (limited to 'CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS') diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino deleted file mode 100644 index 767227b..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino +++ /dev/null @@ -1,56 +0,0 @@ -/* - -MinIMU-9-Arduino-AHRS -Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) - -Copyright (c) 2011 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 Compass_Heading() -{ - float MAG_X; - float MAG_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - - cos_roll = cos(roll); - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range - c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5; - c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5; - c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5; - - // Tilt compensated Magnetic filed X: - MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch; - // Tilt compensated Magnetic filed Y: - MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll; - // Magnetic Heading - MAG_Heading = atan2(-MAG_Y,MAG_X); -} diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino deleted file mode 100644 index 7f9f276..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino +++ /dev/null @@ -1,164 +0,0 @@ -/* - -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]); -} - diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino deleted file mode 100644 index 5ca58e2..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino +++ /dev/null @@ -1,159 +0,0 @@ -/* - -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 . - -*/ - -#ifdef IMU_V5 - -#include -#include - -LSM6 gyro_acc; -LIS3MDL mag; - -#else // older IMUs through v4 - -#include -#include - -L3G gyro; -LSM303 compass; - -#endif - - -void I2C_Init() -{ - Wire.begin(); -} - -void Gyro_Init() -{ -#ifdef IMU_V5 - // Accel_Init() should have already called gyro_acc.init() and enableDefault() - gyro_acc.writeReg(LSM6::CTRL2_G, 0x4C); // 104 Hz, 2000 dps full scale -#else - gyro.init(); - gyro.enableDefault(); - gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale - gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz -#endif -} - -void Read_Gyro() -{ -#ifdef IMU_V5 - gyro_acc.readGyro(); - - AN[0] = gyro_acc.g.x; - AN[1] = gyro_acc.g.y; - AN[2] = gyro_acc.g.z; -#else - gyro.read(); - - AN[0] = gyro.g.x; - AN[1] = gyro.g.y; - AN[2] = gyro.g.z; -#endif - - gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]); - gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]); - gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]); -} - -void Accel_Init() -{ -#ifdef IMU_V5 - gyro_acc.init(); - gyro_acc.enableDefault(); - gyro_acc.writeReg(LSM6::CTRL1_XL, 0x3C); // 52 Hz, 8 g full scale -#else - compass.init(); - compass.enableDefault(); - switch (compass.getDeviceType()) - { - case LSM303::device_D: - compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011 - break; - case LSM303::device_DLHC: - compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode - break; - default: // DLM, DLH - compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11 - } -#endif -} - -// Reads x,y and z accelerometer registers -void Read_Accel() -{ -#ifdef IMU_V5 - gyro_acc.readAcc(); - - AN[3] = gyro_acc.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256) - AN[4] = gyro_acc.a.y >> 4; - AN[5] = gyro_acc.a.z >> 4; -#else - compass.readAcc(); - - AN[3] = compass.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256) - AN[4] = compass.a.y >> 4; - AN[5] = compass.a.z >> 4; -#endif - accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]); - accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]); - accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]); -} - -void Compass_Init() -{ -#ifdef IMU_V5 - mag.init(); - mag.enableDefault(); -#else - // LSM303: doesn't need to do anything because Accel_Init() should have already called compass.enableDefault() -#endif -} - -void Read_Compass() -{ -#ifdef IMU_V5 - mag.read(); - - magnetom_x = SENSOR_SIGN[6] * mag.m.x; - magnetom_y = SENSOR_SIGN[7] * mag.m.y; - magnetom_z = SENSOR_SIGN[8] * mag.m.z; -#else - compass.readMag(); - - magnetom_x = SENSOR_SIGN[6] * compass.m.x; - magnetom_y = SENSOR_SIGN[7] * compass.m.y; - magnetom_z = SENSOR_SIGN[8] * compass.m.z; -#endif -} - diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino deleted file mode 100644 index 5307f5e..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino +++ /dev/null @@ -1,241 +0,0 @@ -/* - -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 . - -*/ - -// Uncomment the following line to use a MinIMU-9 v5 or AltIMU-10 v5. Leave commented for older IMUs (up through v4). -#define IMU_V5 - -// Uncomment the below line to use this axis definition: - // X axis pointing forward - // Y axis pointing to the right - // and Z axis pointing down. -// Positive pitch : nose up -// Positive roll : right wing down -// Positive yaw : clockwise -int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer -// Uncomment the below line to use this axis definition: - // X axis pointing forward - // Y axis pointing to the left - // and Z axis pointing up. -// Positive pitch : nose down -// Positive roll : right wing down -// Positive yaw : counterclockwise -//int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer - -// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168 - -#include - -// accelerometer: 8 g sensitivity -// 3.9 mg/digit; 1 g = 256 -#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer - -#define ToRad(x) ((x)*0.01745329252) // *pi/180 -#define ToDeg(x) ((x)*57.2957795131) // *180/pi - -// gyro: 2000 dps full scale -// 70 mdps/digit; 1 dps = 0.07 -#define Gyro_Gain_X 0.07 //X axis Gyro gain -#define Gyro_Gain_Y 0.07 //Y axis Gyro gain -#define Gyro_Gain_Z 0.07 //Z axis Gyro gain -#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second - -// LSM303/LIS3MDL magnetometer calibration constants; use the Calibrate example from -// the Pololu LSM303 or LIS3MDL library to find the right values for your board - -#define M_X_MIN -1000 -#define M_Y_MIN -1000 -#define M_Z_MIN -1000 -#define M_X_MAX +1000 -#define M_Y_MAX +1000 -#define M_Z_MAX +1000 - -#define Kp_ROLLPITCH 0.02 -#define Ki_ROLLPITCH 0.00002 -#define Kp_YAW 1.2 -#define Ki_YAW 0.00002 - -/*For debugging purposes*/ -//OUTPUTMODE=1 will print the corrected data, -//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) -#define OUTPUTMODE 1 - -#define PRINT_DCM 0 //Will print the whole direction cosine matrix -#define PRINT_ANALOGS 0 //Will print the analog raw data -#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw - -#define STATUS_LED 13 - -float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible - -long timer=0; //general purpuse timer -long timer_old; -long timer24=0; //Second timer used to print values -int AN[6]; //array that stores the gyro and accelerometer data -int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors - -int gyro_x; -int gyro_y; -int gyro_z; -int accel_x; -int accel_y; -int accel_z; -int magnetom_x; -int magnetom_y; -int magnetom_z; -float c_magnetom_x; -float c_magnetom_y; -float c_magnetom_z; -float MAG_Heading; - -float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector -float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector -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}; - -// Euler angles -float roll; -float pitch; -float yaw; - -float errorRollPitch[3]= {0,0,0}; -float errorYaw[3]= {0,0,0}; - -unsigned int counter=0; -byte gyro_sat=0; - -float DCM_Matrix[3][3]= { - { - 1,0,0 } - ,{ - 0,1,0 } - ,{ - 0,0,1 } -}; -float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here - - -float Temporary_Matrix[3][3]={ - { - 0,0,0 } - ,{ - 0,0,0 } - ,{ - 0,0,0 } -}; - -void setup() -{ - Serial.begin(115200); - pinMode (STATUS_LED,OUTPUT); // Status LED - - I2C_Init(); - - Serial.println("Pololu MinIMU-9 + Arduino AHRS"); - - digitalWrite(STATUS_LED,LOW); - delay(1500); - - Accel_Init(); - Compass_Init(); - Gyro_Init(); - - delay(20); - - for(int i=0;i<32;i++) // We take some readings... - { - Read_Gyro(); - Read_Accel(); - for(int y=0; y<6; y++) // Cumulate values - AN_OFFSET[y] += AN[y]; - delay(20); - } - - for(int y=0; y<6; y++) - AN_OFFSET[y] = AN_OFFSET[y]/32; - - AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; - - //Serial.println("Offset:"); - for(int y=0; y<6; y++) - Serial.println(AN_OFFSET[y]); - - delay(2000); - digitalWrite(STATUS_LED,HIGH); - - timer=millis(); - delay(20); - counter=0; -} - -void loop() //Main Loop -{ - if((millis()-timer)>=20) // Main loop runs at 50Hz - { - counter++; - timer_old = timer; - timer=millis(); - if (timer>timer_old) - { - G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) - if (G_Dt > 0.2) - G_Dt = 0; // ignore integration times over 200 ms - } - else - G_Dt = 0; - - - - // *** DCM algorithm - // Data adquisition - Read_Gyro(); // This read gyro data - Read_Accel(); // Read I2C accelerometer - - if (counter > 5) // Read compass data at 10Hz... (5 loop runs) - { - counter=0; - Read_Compass(); // Read I2C magnetometer - Compass_Heading(); // Calculate magnetic heading - } - - // Calculations... - Matrix_update(); - Normalize(); - Drift_correction(); - Euler_angles(); - // *** - - printdata(); - } - -} diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino deleted file mode 100644 index f1f31e5..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino +++ /dev/null @@ -1,91 +0,0 @@ -/* - -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 printdata(void) -{ - Serial.print("!"); - - #if PRINT_EULER == 1 - Serial.print("ANG:"); - Serial.print(ToDeg(roll)); - Serial.print(","); - Serial.print(ToDeg(pitch)); - Serial.print(","); - Serial.print(ToDeg(yaw)); - #endif - #if PRINT_ANALOGS==1 - Serial.print(",AN:"); - Serial.print(AN[0]); //(int)read_adc(0) - Serial.print(","); - Serial.print(AN[1]); - Serial.print(","); - Serial.print(AN[2]); - Serial.print(","); - Serial.print(AN[3]); - Serial.print (","); - Serial.print(AN[4]); - Serial.print (","); - Serial.print(AN[5]); - Serial.print(","); - Serial.print(c_magnetom_x); - Serial.print (","); - Serial.print(c_magnetom_y); - Serial.print (","); - Serial.print(c_magnetom_z); - #endif - #if PRINT_DCM == 1 - Serial.print (",DCM:"); - Serial.print(DCM_Matrix[0][0]); - Serial.print (","); - Serial.print(DCM_Matrix[0][1]); - Serial.print (","); - Serial.print(DCM_Matrix[0][2]); - Serial.print (","); - Serial.print(DCM_Matrix[1][0]); - Serial.print (","); - Serial.print(DCM_Matrix[1][1]); - Serial.print (","); - Serial.print(DCM_Matrix[1][2]); - Serial.print (","); - Serial.print(DCM_Matrix[2][0]); - Serial.print (","); - Serial.print(DCM_Matrix[2][1]); - Serial.print (","); - Serial.print(DCM_Matrix[2][2]); - #endif - Serial.println(); - -} - -/*long convert_to_dec(float x) -{ - return x*10000000; -}*/ - diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino deleted file mode 100644 index d69b49c..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino +++ /dev/null @@ -1,70 +0,0 @@ -/* - -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 . - -*/ - -//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]; - } -} - - - diff --git a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/matrix.ino b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/matrix.ino deleted file mode 100644 index c8a7a15..0000000 --- a/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/matrix.ino +++ /dev/null @@ -1,49 +0,0 @@ -/* - -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 . - -*/ - -/**************************************************/ -//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]) -{ - for(int x = 0; x < 3; x++) - { - for(int y = 0; y < 3; y++) - { - mat[x][y] = 0; - - for(int w = 0; w < 3; w++) - { - mat[x][y] += a[x][w] * b[w][y]; - } - } - } -} - - -- cgit v1.2.3