From 5340707b87937d62c6d72197cf494d67851d05e3 Mon Sep 17 00:00:00 2001 From: abel Date: Tue, 8 Oct 2024 17:41:29 +0200 Subject: UPDATE FOLDER STUCTURE MOET APARTE_PCB NOG VERWIJDERTEN WINDOWS HEEFT BEST VERGRENDELD --- .../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 insertions(+) create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/DCM.ino create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino create mode 100644 CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino create 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 new file mode 100644 index 0000000..767227b --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Compass.ino @@ -0,0 +1,56 @@ +/* + +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 new file mode 100644 index 0000000..7f9f276 --- /dev/null +++ b/CODE/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]); +} + 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 new file mode 100644 index 0000000..5ca58e2 --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino @@ -0,0 +1,159 @@ +/* + +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 new file mode 100644 index 0000000..5307f5e --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/MinIMU9AHRS.ino @@ -0,0 +1,241 @@ +/* + +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 new file mode 100644 index 0000000..f1f31e5 --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Output.ino @@ -0,0 +1,91 @@ +/* + +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 new file mode 100644 index 0000000..d69b49c --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/Vector.ino @@ -0,0 +1,70 @@ +/* + +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 new file mode 100644 index 0000000..c8a7a15 --- /dev/null +++ b/CODE/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/matrix.ino @@ -0,0 +1,49 @@ +/* + +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